diff --git a/KugleFirmware/KugleFirmware Debug.cfg b/KugleFirmware/KugleFirmware Debug.cfg
index 2ca9335..985835b 100644
--- a/KugleFirmware/KugleFirmware Debug.cfg	
+++ b/KugleFirmware/KugleFirmware Debug.cfg	
@@ -19,7 +19,7 @@ set ENABLE_LOW_POWER 1
 set STOP_WATCHDOG 1
 
 # STlink Debug clock frequency
-set CLOCK_FREQ 4000
+set CLOCK_FREQ 8000
 
 # use hardware reset, connect under reset
 # connect_assert_srst needed if low power mode application running (WFI...)
diff --git a/KugleFirmware/KugleFirmware.xml b/KugleFirmware/KugleFirmware.xml
index 931b7ee..7f6c4f2 100644
--- a/KugleFirmware/KugleFirmware.xml
+++ b/KugleFirmware/KugleFirmware.xml
@@ -1,9 +1,19 @@
 <?xml version="1.0" encoding="UTF-8"?>
-<targetDefinitions xmlns="http://openstm32.org/stm32TargetDefinitions" xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xsi:schemaLocation="http://openstm32.org/stm32TargetDefinitions stm32TargetDefinitions.xsd">
-  <board id="KugleFirmware">
+<!DOCTYPE targetDefinitions [ 
+	<!ELEMENT targetDefinitions (board)>
+	<!ELEMENT board (name, dbgIF+, dbgDEV, mcuId)>
+	<!ELEMENT name (#PCDATA)>
+	<!ELEMENT dbgIF (#PCDATA)>
+	<!ELEMENT dbgDEV (#PCDATA)>
+	<!ELEMENT mcuId (#PCDATA)>
+	<!ATTLIST board id CDATA #REQUIRED>
+]>
+
+<targetDefinitions>
+  <board id="kuglefirmware">
     <name>KugleFirmware</name>
-    <mcuId>STM32H743ZITx</mcuId>  <!-- mcu-->
     <dbgIF>SWD</dbgIF>
-    <dbgDEV>ST-LinkV2-1</dbgDEV>
+    <dbgDEV>ST-Link</dbgDEV>
+    <mcuId>stm32h743zitx</mcuId>
   </board>
 </targetDefinitions>
diff --git a/KugleFirmware/Libraries/Devices/Battery/Battery.cpp b/KugleFirmware/Libraries/Devices/Battery/Battery.cpp
index c51e88b..06aeb59 100644
--- a/KugleFirmware/Libraries/Devices/Battery/Battery.cpp
+++ b/KugleFirmware/Libraries/Devices/Battery/Battery.cpp
@@ -17,13 +17,318 @@
  */
  
 #include "Battery.h"
+#include <bitset>         // std::bitset
+#include "Debug.h"
  
-Battery::Battery()
+Battery::Battery(SMBus::port_t SMBusHardwareResources)
 {
-	
+	location = 0;
+	smbus = new SMBus(SMBusHardwareResources, DeviceAddr);
+	present = smbus->devicePresent();
+}
+
+Battery::Battery(SMBus::port_t SMBusHardwareResources,uint8_t _location)
+{
+	location = _location;
+	smbus = new SMBus(SMBusHardwareResources, DeviceAddr);
+	present = smbus->devicePresent();
 }
 
 Battery::~Battery()
 {
-	
+	if(smbus){
+		delete smbus;
+	}
+}
+
+
+
+// SMBus Commands
+// -------------------------------------------------------------------------------------------
+// Reading and writing from/to the battery.
+//
+/* This file contains the following functions:
+    SMBus_R(); Low level function used to read on the SMBus
+    SMBus_w(); Low level function used to write to the SMBus
+    getCurrent(); Returns the status of the bus and saves the current in a buffer
+    getCycleCount(); Returns the status of the bus and saves the number of Cycles in a buffer
+    getRunTimeToEmpty(); Returns the status of the bus and saves how much battery time there is left in a buffer
+    getTemperature(); Returns the status of the bus and saves the current Temperature in a buffer
+    getVoltage(); Returns the status of the bus and saves the Voltage in a buffer
+    SetChargeCurrentLimit(); Changes the Charge Current Limit
+    SetInputCurrentLimit(); Changes the Input Current limit
+
+Look under the specific function for more information
+*/
+// -------------------------------------------------------------------------------------------
+
+float Battery::getCurrent(void){
+	if(smbus->devicePresent()){
+		// Returns the current being supplied (or accepted) through the battery's terminals.
+		uint8_t const N_bytes = 2;
+		uint8_t responbuf_tmp[N_bytes];
+
+		smbus->Read(CmdAddr::read::Current,responbuf_tmp,N_bytes);
+
+		int16_t number = responbuf_tmp[0] | responbuf_tmp[1] << 8;
+		float current = ((float)number) / 1000;
+
+		return current;
+	}
+	else{
+		return 0;
+	}
+}
+
+float Battery::getTemperature(void){
+	if(smbus->devicePresent()){
+		// Returns the pack's internal temperature.
+		uint8_t N_bytes = 2;
+		uint8_t responbuf_tmp[2];
+
+		smbus->Read(CmdAddr::read::Temperature,responbuf_tmp,N_bytes);
+
+		int number = responbuf_tmp[0] | responbuf_tmp[1] << 8;
+		float temperature = ((float)number) / 10 - 273.15;
+
+		return temperature;
+	}
+	else{
+		return 0;
+	}
+}
+
+float Battery::getVoltage(void){
+	if(smbus->devicePresent()){
+		// Returns the battery's voltage (measured at the cell stack)
+		uint8_t N_bytes = 2;
+		uint8_t responbuf_tmp[2];
+
+		smbus->Read(CmdAddr::read::Voltage,responbuf_tmp,N_bytes);
+
+		int16_t number = responbuf_tmp[0] | responbuf_tmp[1] << 8;
+		float voltage = ((float)number) / 1000;
+
+		return voltage;
+	}
+	else{
+		return 0;
+	}
+}
+
+float Battery::getCharge(void)           // Current charge in Ah  (If unmeasured NaN)
+{
+	if(smbus->devicePresent()){
+		// Returns the battery's charge
+		uint8_t N_bytes = 2;
+		uint8_t responbuf_tmp[2];
+
+		smbus->Read(CmdAddr::read::RemainingCapacity,responbuf_tmp,N_bytes);
+
+		uint16_t number = responbuf_tmp[0] | responbuf_tmp[1] << 8;
+		float charge = ((float)number) / 1000;
+
+		return charge;
+	}
+	else{
+		return 0;
+	}
+}
+
+
+float Battery::getCapacity(void)         // Capacity in Ah (last full capacity)  (If unmeasured NaN)
+{
+	if(smbus->devicePresent()){
+		// Returns the battery's capacity
+		uint8_t N_bytes = 2;
+		uint8_t responbuf_tmp[2];
+
+		smbus->Read(CmdAddr::read::FullChargeCapacity,responbuf_tmp,N_bytes);
+
+		uint16_t number = responbuf_tmp[0] | responbuf_tmp[1] << 8;
+		float Capacity = ((float)number) / 1000;
+
+		return Capacity;
+	}
+	else{
+		return 0;
+	}
 }
+
+float Battery::getDesign_capacity(void)  // Capacity in Ah (design capacity)  (If unmeasured NaN)
+{
+	if(smbus->devicePresent()){
+		// Returns the battery's Design Capacity
+		uint8_t N_bytes = 2;
+		uint8_t responbuf_tmp[2];
+
+		smbus->Read(CmdAddr::read::DesignCapacity,responbuf_tmp,N_bytes);
+
+		uint16_t number = responbuf_tmp[0] | responbuf_tmp[1] << 8;
+		float DesignCapacity = ((float)number) / 1000;
+
+		return DesignCapacity;
+	}
+	else{
+		return 0;
+	}
+}
+
+float Battery::getPercentage(void)       // Charge percentage on 0 to 1 range  (If unmeasured NaN)
+{
+	if(smbus->devicePresent()){
+		// Returns the battery's charge percentage
+		uint8_t N_bytes = 2;
+		uint8_t responbuf_tmp[2];
+
+		smbus->Read(CmdAddr::read::RelativeStateOfCharge,responbuf_tmp,N_bytes);
+
+		uint16_t number = responbuf_tmp[0] | responbuf_tmp[1] << 8;
+		float chargePercentage = ((float)number) / 100;
+
+		return chargePercentage;
+	}
+	else{
+		return 0;
+	}
+}
+
+uint8_t   Battery::getPowerSupplyStatus(void)     // The charging status as reported. Values defined above
+{
+	if(smbus->devicePresent()){
+		// Returns the battery's status
+		uint8_t N_bytes = 2;
+		uint8_t responbuf_tmp[2];
+
+		smbus->Read(CmdAddr::read::BatteryStatus,responbuf_tmp,N_bytes);
+
+		uint16_t number = responbuf_tmp[0] | responbuf_tmp[1] << 8;
+		std::bitset<16> tmp(number);
+
+		if (tmp.test(6)) return POWER_SUPPLY_STATUS_DISCHARGING; // Discharging??
+		else if (tmp.test(5)) return POWER_SUPPLY_STATUS_FULL; // fully charged?
+		else return POWER_SUPPLY_STATUS_UNKNOWN;
+	}
+	else{
+		return POWER_SUPPLY_STATUS_UNKNOWN;
+	}
+}
+
+uint8_t   Battery::getPowerSupplyHealth(void)     // The battery health metric. Values defined above
+{
+	if(smbus->devicePresent()){
+		// Returns the battery's health
+		uint8_t N_bytes = 2;
+		uint8_t responbuf_tmp[2];
+
+		smbus->Read(CmdAddr::read::BatteryStatus,responbuf_tmp,N_bytes);
+
+		uint16_t number = responbuf_tmp[0] | responbuf_tmp[1] << 8;
+		std::bitset<16> tmp(number);
+
+		if (tmp.test(12)) return POWER_SUPPLY_HEALTH_OVERHEAT;
+		else if (tmp.test(15)) return POWER_SUPPLY_HEALTH_UNSPEC_FAILURE;
+		else if (tmp.test(14)) return POWER_SUPPLY_HEALTH_UNSPEC_FAILURE;
+		else if (tmp.test(11)) return POWER_SUPPLY_HEALTH_UNSPEC_FAILURE;
+		else if (tmp.test(9)) return POWER_SUPPLY_HEALTH_UNSPEC_FAILURE;
+		else if (tmp.test(8)) return POWER_SUPPLY_HEALTH_UNSPEC_FAILURE;
+		else return POWER_SUPPLY_HEALTH_GOOD;
+	}
+	else{
+		return POWER_SUPPLY_HEALTH_UNKNOWN;
+	}
+}
+
+uint8_t   Battery::getPowerSupplyTechnology(void) // The battery chemistry. Values defined above
+{
+	if(smbus->devicePresent()){
+		uint8_t N_bytes = 10;
+		uint8_t responbuf_tmp[N_bytes];
+
+		uint8_t bytesRead = smbus->blockRead(CmdAddr::read::DeviceChemistry,responbuf_tmp,N_bytes);
+
+		char responbuf_tmp2[bytesRead+1];
+
+		for (int i = 0; i<bytesRead;i++){
+			responbuf_tmp2[i] = toupper((char)responbuf_tmp[i]);
+		}
+		responbuf_tmp2[bytesRead] = {'\0'};
+
+		if (strcmp(responbuf_tmp2, "LION") == 0){return POWER_SUPPLY_TECHNOLOGY_LION;}
+		else if (strcmp(responbuf_tmp2, "NIMH") == 0){return POWER_SUPPLY_TECHNOLOGY_NIMH;}
+		else if (strcmp(responbuf_tmp2, "NICD") == 0){return POWER_SUPPLY_TECHNOLOGY_NICD;}
+		else if (strcmp(responbuf_tmp2, "LIP") == 0){return POWER_SUPPLY_TECHNOLOGY_LIPO;}
+		else{return POWER_SUPPLY_TECHNOLOGY_UNKNOWN;}
+	}
+	else{
+		return 0;
+	}
+}
+
+bool    Battery::checkPresent(void)        // True if the battery is present
+{
+	return present;
+}
+
+uint8_t Battery::getLocation(void)        // The location into which the battery is inserted. (slot number or plug)
+{
+	return location;
+}
+
+uint32_t Battery::getSerialNumber(void)
+{
+	if(smbus->devicePresent()){
+		// Returns the battery's serial number
+
+		/* Explanation
+		 * SerialNumber(): This function is used to return a serial number. This number when combined with the ManufacturerName(),
+		 * the DeviceName(), and the ManufactureDate() will uniquely identify the battery (unsigned int).
+		 *
+		 * Since we already know the manufacture and device name (just look at it) we return a combination of ManufactureDate() and SerialNumber()
+		 * to uniqly identify the battery!
+		*/
+
+
+		uint8_t N_bytes = 2;
+		uint8_t responbuf_tmp[2];
+
+		smbus->Read(CmdAddr::read::ManufactureDate,responbuf_tmp,N_bytes);
+
+		uint16_t ManufactureDate = responbuf_tmp[0] | responbuf_tmp[1] << 8;
+
+		smbus->Read(CmdAddr::read::SerialNumber,responbuf_tmp,N_bytes);
+
+		uint16_t serialNumber = responbuf_tmp[0] | responbuf_tmp[1] << 8;
+
+		uint32_t uniqueIdentifier = ManufactureDate << 16 | serialNumber;
+		return uniqueIdentifier;
+	}
+	else{
+		return 0;
+	}
+}
+
+uint16_t Battery::getCycleCount(void){
+	if(smbus->devicePresent()){
+		// Returns the number of charge/discharge cycles the battery has experienced. A charge/discharge cycle is defined as: an amount of discharge approximately equal to the value of DesignCapacity.
+		uint8_t N_bytes = 2;
+		uint8_t responbuf_tmp[2];
+
+		smbus->Read(CmdAddr::read::CycleCount,responbuf_tmp,N_bytes);
+
+		uint16_t nCycle = responbuf_tmp[0] | responbuf_tmp[1] << 8;
+
+		return nCycle;
+	}
+	else{
+		return 0;
+	}
+}
+
+bool Battery::isBatteryGettingOld(void){
+	if (getCycleCount() > gettingOldCycleCount){
+		return true;
+	}
+	else{return false;}
+}
+
diff --git a/KugleFirmware/Libraries/Devices/Battery/Battery.h b/KugleFirmware/Libraries/Devices/Battery/Battery.h
index 7d15cf5..f4dde9a 100644
--- a/KugleFirmware/Libraries/Devices/Battery/Battery.h
+++ b/KugleFirmware/Libraries/Devices/Battery/Battery.h
@@ -19,15 +19,132 @@
 #ifndef DEVICES_BATTERY_H
 #define DEVICES_BATTERY_H
 
+#include "SMBus.h"
+
 class Battery
 {
 
 public:
-	Battery();	
+	Battery(SMBus::port_t SMBusHardwareResources,uint8_t _location);
+	Battery(SMBus::port_t SMBusHardwareResources);
 	~Battery();
 
+	float getCurrent(void);
+	float getTemperature(void);
+	float getVoltage(void);
+
+
+	float getCharge(void);           // Current charge in Ah  (If unmeasured NaN)
+	float getCapacity(void);         // Capacity in Ah (last full capacity)  (If unmeasured NaN)
+	float getDesign_capacity(void);  // Capacity in Ah (design capacity)  (If unmeasured NaN)
+	float getPercentage(void);       // Charge percentage on 0 to 1 range  (If unmeasured NaN)
+	uint8_t   getPowerSupplyStatus(void);     // The charging status as reported. Values defined above
+	uint8_t   getPowerSupplyHealth(void);     // The battery health metric. Values defined above
+	uint8_t   getPowerSupplyTechnology(void); // The battery chemistry. Values defined above
+	bool    checkPresent(void);        // True if the battery is present
+	uint16_t getCycleCount(void);
+
+	//float cell_voltage[10];   // An array of individual cell voltages for each cell in the pack
+						   // If individual voltages unknown but number of cells known set each to NaN
+	uint8_t getLocation(void);        // The location into which the battery is inserted. (slot number or plug)
+	uint32_t getSerialNumber(void);   // The best approximation of the battery serial number
+	bool isBatteryGettingOld(void);   // The best approximation of the battery serial number
+
+	enum {
+	    POWER_SUPPLY_STATUS_UNKNOWN = 0u,
+	    POWER_SUPPLY_STATUS_CHARGING = 1u,
+	    POWER_SUPPLY_STATUS_DISCHARGING = 2u,
+	    POWER_SUPPLY_STATUS_NOT_CHARGING = 3u,
+	    POWER_SUPPLY_STATUS_FULL = 4u,
+	    POWER_SUPPLY_HEALTH_UNKNOWN = 0u,
+	    POWER_SUPPLY_HEALTH_GOOD = 1u,
+	    POWER_SUPPLY_HEALTH_OVERHEAT = 2u,
+	    POWER_SUPPLY_HEALTH_DEAD = 3u,
+	    POWER_SUPPLY_HEALTH_OVERVOLTAGE = 4u,
+	    POWER_SUPPLY_HEALTH_UNSPEC_FAILURE = 5u,
+	    POWER_SUPPLY_HEALTH_COLD = 6u,
+	    POWER_SUPPLY_HEALTH_WATCHDOG_TIMER_EXPIRE = 7u,
+	    POWER_SUPPLY_HEALTH_SAFETY_TIMER_EXPIRE = 8u,
+	    POWER_SUPPLY_TECHNOLOGY_UNKNOWN = 0u,
+	    POWER_SUPPLY_TECHNOLOGY_NIMH = 1u,
+	    POWER_SUPPLY_TECHNOLOGY_LION = 2u,
+	    POWER_SUPPLY_TECHNOLOGY_LIPO = 3u,
+	    POWER_SUPPLY_TECHNOLOGY_LIFE = 4u,
+	    POWER_SUPPLY_TECHNOLOGY_NICD = 5u,
+	    POWER_SUPPLY_TECHNOLOGY_LIMN = 6u,
+	};
+
 private:
-	
+	SMBus *smbus;
+	uint8_t location; // the idx of the SMBus that the battery is attached to 0 for unknown
+	uint8_t present;
+	uint8_t DeviceAddr = 0x0B; //0x16
+	uint16_t gettingOldCycleCount = 300;
+
+	class CmdAddr
+	{
+	public:
+		class read{
+		public:
+			enum : uint8_t
+			{ // according to smbus protocol v 1.1
+				ManufacturerAccess = 0x00,
+				RemainingCapacityAlarm,
+				RemainingTimeAlarm,
+				BatteryMode,
+				AtRate,
+				AtRateTimeToFull,
+				AtRateTimeToEmpty,
+				AtRateOK,
+				Temperature,
+				Voltage,
+				Current,
+				AverageCurrent,
+				MaxError,
+				RelativeStateOfCharge,
+				AbsoluteStateOfCharge,
+				RemainingCapacity,
+				FullChargeCapacity,
+				RunTimeToEmpty,
+				AverageTimeToEmpty,
+				AverageTimeToFull,
+				ChargingCurrent,
+				ChargingVoltage,
+				BatteryStatus,
+				CycleCount,
+				DesignCapacity,
+				DesignVoltage,
+				SpecificationInfo,
+				ManufactureDate,
+				SerialNumber,
+				// 0x1d - 0x1f reserved
+				ManufacturerName = 0x20,
+				DeviceName,
+				DeviceChemistry,
+				ManufacturerData
+				// 0x25 - 0x2e reserved
+				// 0x2f OptionalMfgFunction5
+				// 0x30 - 0x3b reserved
+				// 0x3c - 0x3f OptionalMfgFunction4-1
+			};
+		};
+
+		class write{
+		public:
+			enum : uint8_t
+			{ // according to smbus protocol v 1.1
+				ManufacturerAccess = 0x00,
+				RemainingCapacityAlarm,
+				RemainingTimeAlarm,
+				BatteryMode
+				// 0x25 - 0x2e reserved
+				// 0x2f OptionalMfgFunction5
+				// 0x30 - 0x3b reserved
+				// 0x3c - 0x3f OptionalMfgFunction4-1
+			};
+		};
+	};
+
 };
 	
 	
diff --git a/KugleFirmware/Libraries/Devices/BatteryBoard/BatteryBoard.cpp b/KugleFirmware/Libraries/Devices/BatteryBoard/BatteryBoard.cpp
new file mode 100644
index 0000000..1dd29f8
--- /dev/null
+++ b/KugleFirmware/Libraries/Devices/BatteryBoard/BatteryBoard.cpp
@@ -0,0 +1,139 @@
+/* Copyright (C) 2018-2019 Thomas Jespersen, TKJ Electronics. All rights reserved.
+ *
+ * This program is free software: you can redistribute it and/or modify  
+ * it under the terms of the GNU General Public License as published by  
+ * the Free Software Foundation, version 3.
+ *
+ * This program is distributed in the hope that it will be useful, but 
+ * WITHOUT ANY WARRANTY; without even the implied warranty of 
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 
+ * General Public License for more details. 
+ *
+ * Contact information
+ * ------------------------------------------
+ * Thomas Jespersen, TKJ Electronics
+ * Web      :  http://www.tkjelectronics.dk
+ * e-mail   :  thomasj@tkjelectronics.dk
+ * ------------------------------------------
+ */
+
+#include "BatteryBoard.h"
+#include "SMBus.h"
+#include "Debug.h"
+
+BatteryBoard::BatteryBoard(SMBus::port_t SMBusHardwareResources, uint16_t _chargerCurrentLimit) : chargerCurrentLimit(_chargerCurrentLimit), batteryBoardRamTaskHandle(0), batteryBoardEepromTaskHandle(0)
+{
+
+	// the device can only be accessed when the charger is plugged in!
+	smbus = new SMBus(SMBusHardwareResources, DeviceAddr);
+
+	SetCurrentLimitRAM(chargerCurrentLimit); // set the input limit of the batteryboard to whatever the charger can deliver!
+	//SetCurrentLimitEEPROM(chargerCurrentLimit); // set the input limit of the batteryboard to whatever the charger can deliver!
+
+	// We do this in RAM for 2 reasons!
+	// 1) the EEPROM have limited erase/write cycles
+	// 2) If the change was saved to EEPROM it would only take affect after all sources of power to the battery board is removed (thus it requires physical interaction with the robot, e.g. removing the battery and charger cable)
+	// Therefore a change the the setting
+}
+
+BatteryBoard::~BatteryBoard()
+{
+	if(smbus){delete smbus;}
+	if(batteryBoardRamTaskHandle){vTaskDelete(batteryBoardRamTaskHandle);}
+	if(batteryBoardEepromTaskHandle){vTaskDelete(batteryBoardEepromTaskHandle);}
+}
+
+void BatteryBoard::BatteryBoardRAMThread(void * pvParameters)
+{
+	BatteryBoard * thisObjPtr = (BatteryBoard *)pvParameters;
+
+	while(1){ // loop until the device becomes available and the configuration can be changed
+		if(thisObjPtr->SetCurrentLimitRAM()){ // stop task
+			if (thisObjPtr->batteryBoardRamTaskHandle){ // free memory associated with the task
+				vTaskSuspend(thisObjPtr->batteryBoardRamTaskHandle);
+			}
+		}
+		osDelay(1000);
+	}
+}
+
+void BatteryBoard::BatteryBoardEEPROMThread(void * pvParameters)
+{
+	BatteryBoard * thisObjPtr = (BatteryBoard *)pvParameters;
+
+	while(1){ // loop until the device becomes available and the configuration can be changed
+		if(thisObjPtr->SetCurrentLimitEEPROM()){ // stop task
+			if (thisObjPtr->batteryBoardEepromTaskHandle){ // free memory associated with the task
+				vTaskSuspend(thisObjPtr->batteryBoardEepromTaskHandle);
+			}
+		}
+		osDelay(1000);
+	}
+}
+
+bool BatteryBoard::SetCurrentLimitRAM(){
+	if(smbus->devicePresent()){
+		SetChargeCurrentLimit(currentLimitRAM,configuration::RAM);
+		osDelay(5);
+		SetInputCurrentLimit(currentLimitRAM,configuration::RAM);
+		return true;
+	}
+	else{return false;}
+}
+
+bool BatteryBoard::SetCurrentLimitEEPROM(){
+	if(smbus->devicePresent()){
+		SetChargeCurrentLimit(currentLimitEEPROM,configuration::EEPROM);
+		osDelay(5);
+		SetInputCurrentLimit(currentLimitEEPROM,configuration::EEPROM);
+		return true;
+	}
+	else{return false;}
+}
+
+
+void BatteryBoard::SetCurrentLimitRAM(uint16_t LimitmA){
+	currentLimitRAM = LimitmA;
+	if (!batteryBoardRamTaskHandle){
+		xTaskCreate(BatteryBoard::BatteryBoardRAMThread, (char *)"Battery Board RAM Change", BATTERY_BOARD_THREAD_STACK, (void*) this, BATTERY_BOARD_PRIORITY, &batteryBoardRamTaskHandle);
+	}
+	else{
+		vTaskResume(batteryBoardRamTaskHandle);
+	}
+}
+
+void BatteryBoard::SetCurrentLimitEEPROM(uint16_t LimitmA){
+	currentLimitEEPROM = LimitmA;
+	if (!batteryBoardEepromTaskHandle){
+		xTaskCreate(BatteryBoard::BatteryBoardEEPROMThread, (char *)"Battery Board EEPROM Change", BATTERY_BOARD_THREAD_STACK, (void*) this, BATTERY_BOARD_PRIORITY, &batteryBoardEepromTaskHandle);
+	}
+	else{vTaskResume(batteryBoardEepromTaskHandle);}
+}
+
+void BatteryBoard::SetChargeCurrentLimit(uint16_t LimitmA, uint8_t config){
+	// Sets the Charge Current Limit
+	// LimitmA: The current limit in mA
+
+	uint8_t writeLength = 4;
+	uint8_t cmdBuffer[writeLength];
+	cmdBuffer[0] = writeLength-1;
+	cmdBuffer[1] = config;
+	cmdBuffer[2] = (uint8_t)((LimitmA & 0xFF00U) >> 8U);     // First 8 bits of LimitmA
+	cmdBuffer[3] = (uint8_t)(LimitmA & 0x00FFU);           // Last 8 bit of LimitmA
+
+	smbus->Write(CmdAddr::write::SetChargeCurrentLimit,cmdBuffer,writeLength); // we use a normal "write word" function to make a "write block" call
+}
+
+void BatteryBoard::SetInputCurrentLimit(uint16_t LimitmA, uint8_t config){
+	// Sets the Charge Current Limit
+	// LimitmA: The current limit in mA
+
+	uint8_t writeLength = 4;
+	uint8_t cmdBuffer[writeLength];
+	cmdBuffer[0] = writeLength-1;
+	cmdBuffer[1] = config;
+	cmdBuffer[2] = (uint8_t)((LimitmA & 0xFF00U) >> 8U);     // First 8 bits of LimitmA
+	cmdBuffer[3] = (uint8_t)(LimitmA & 0x00FFU);           // Last 8 bit of LimitmA
+
+	smbus->Write(CmdAddr::write::SetInputCurrentLimit,cmdBuffer,writeLength);
+}
diff --git a/KugleFirmware/Libraries/Devices/BatteryBoard/BatteryBoard.h b/KugleFirmware/Libraries/Devices/BatteryBoard/BatteryBoard.h
new file mode 100644
index 0000000..7052519
--- /dev/null
+++ b/KugleFirmware/Libraries/Devices/BatteryBoard/BatteryBoard.h
@@ -0,0 +1,101 @@
+/* Copyright (C) 2018-2019 Thomas Jespersen, TKJ Electronics. All rights reserved.
+ *
+ * This program is free software: you can redistribute it and/or modify  
+ * it under the terms of the GNU General Public License as published by  
+ * the Free Software Foundation, version 3.
+ *
+ * This program is distributed in the hope that it will be useful, but 
+ * WITHOUT ANY WARRANTY; without even the implied warranty of 
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 
+ * General Public License for more details. 
+ *
+ * Contact information
+ * ------------------------------------------
+ * Thomas Jespersen, TKJ Electronics
+ * Web      :  http://www.tkjelectronics.dk
+ * e-mail   :  thomasj@tkjelectronics.dk
+ * ------------------------------------------
+ */
+ 
+#ifndef DEVICES_BATTERYBOARD_H
+#define DEVICES_BATTERYBOARD_H
+
+#include "SMBus.h"
+
+class BatteryBoard
+{
+
+public:
+	BatteryBoard(SMBus::port_t SMBusHardwareResources, uint16_t _chargerCurrentLimit);
+	~BatteryBoard();
+	void SetCurrentLimitRAM(uint16_t LimitmA);
+	void SetCurrentLimitEEPROM(uint16_t LimitmA);
+
+private:
+	static void BatteryBoardRAMThread(void * pvParameters);
+	static void BatteryBoardEEPROMThread(void * pvParameters);
+	bool SetCurrentLimitRAM();
+	bool SetCurrentLimitEEPROM();
+	void SetChargeCurrentLimit(uint16_t LimitmA, uint8_t configuration);
+	void SetInputCurrentLimit(uint16_t LimitmA, uint8_t configuration);
+
+	SMBus * smbus;
+	uint16_t chargerCurrentLimit; // mA
+	uint16_t currentLimitEEPROM;
+	uint16_t currentLimitRAM;
+	uint8_t DeviceAddr = 0x10;
+
+	// task params
+	const uint32_t BATTERY_BOARD_THREAD_STACK = 256;
+
+	TaskHandle_t batteryBoardRamTaskHandle;
+	TaskHandle_t batteryBoardEepromTaskHandle;
+
+	class CmdAddr
+	{
+	public:
+		class read{
+		public:
+			enum : uint8_t
+			{ // according to smbus protocol v 1.1
+				ManufacturerAccess = 0x00,
+				RemainingCapacityAlarm,
+				RemainingTimeAlarm,
+				BatteryMode
+				// 0x25 - 0x2e reserved
+				// 0x2f OptionalMfgFunction5
+				// 0x30 - 0x3b reserved
+				// 0x3c - 0x3f OptionalMfgFunction4-1
+			};
+		};
+
+		class write{
+		public:
+			enum : uint8_t
+			{ // according to smbus protocol v 1.1
+				ChargerMode = 0x12,
+				ChargingCurrent = 0x14,
+				ChargingVoltage,
+				AlarmWarning,
+				SelectorPresets = 0x22,
+				// 0x3c - 0x3f OptionalMfgFunction4-1
+				// PMM240 specific commands...
+				SetChargeCurrentLimit = 0x3C,
+				SetInputCurrentLimit = 0x3D,
+			};
+		};
+	};
+	
+	class configuration{
+	public:
+		enum : uint8_t
+		{ // according to smbus protocol v 1.1
+			RAM = 0x00,
+			EEPROM = 0x01
+		};
+	};
+
+};
+	
+	
+#endif
diff --git a/KugleFirmware/Libraries/Devices/LSPC/MessageTypes.h b/KugleFirmware/Libraries/Devices/LSPC/MessageTypes.h
index bd20f23..6d11dca 100644
--- a/KugleFirmware/Libraries/Devices/LSPC/MessageTypes.h
+++ b/KugleFirmware/Libraries/Devices/LSPC/MessageTypes.h
@@ -373,6 +373,7 @@ namespace lspc
             AttitudeControllerInfo = 0x13,
             VelocityControllerInfo = 0x14,
             ControllerDebug = 0x15,
+			powerManagment_info = 0x16,
             MPCinfo = 0x20,
             PredictedMPCtrajectory = 0x21,
             RawSensor_IMU_MPU9250 = 0x30,
@@ -627,15 +628,36 @@ namespace lspc
 
         typedef struct
         {
-            float time;
-            float vbat1;
-            float vbat2;
-            float current1;
-            float current2;
-            float pct1;
-            float pct2;
+            //Header  header
+            float voltage;          // Voltage in Volts (Mandatory)
+            float current;          // Negative when discharging (A)  (If unmeasured NaN)
+            float charge;           // Current charge in Ah  (If unmeasured NaN)
+            float capacity;         // Capacity in Ah (last full capacity)  (If unmeasured NaN)
+            float design_capacity;  // Capacity in Ah (design capacity)  (If unmeasured NaN)
+            float percentage;       // Charge percentage on 0 to 1 range  (If unmeasured NaN)
+			uint8_t   power_supply_status;     // The charging status as reported. Values defined above
+			uint8_t   power_supply_health;     // The battery health metric. Values defined above
+			uint8_t   power_supply_technology; // The battery chemistry. Values defined above
+            bool    present;        // True if the battery is present
+
+            float cell_voltage[10];   // An array of individual cell voltages for each cell in the pack
+                                   // If individual voltages unknown but number of cells known set each to NaN
+            uint8_t location;        // The location into which the battery is inserted. (slot number or plug)
+            uint32_t serial_number;   // The best approximation of the battery serial number
         } RawSensor_Battery_t;
 
+        typedef struct
+        {
+        	float time;
+        	float batteryAssamblyChargePercentage; // Charge percentage on 0 to 1 range  (If unmeasured NaN)
+        	float batteryAssamblyTotalCharge; // Current charge in Ah  (If unmeasured NaN)
+        	float batteryAssamblyTotalCapacity; // Capacity in Ah (last full capacity)  (If unmeasured NaN)
+        	uint8_t nBatteries; // 0 if no batteries present
+			uint8_t power_supply_status;     // The charging status as reported.
+			uint8_t power_supply_health;     // The battery health metric.
+			bool batteryAssamblyGettingOld;
+        } powerManagment_info_t;
+
         typedef struct
         {
             bool acknowledged;
diff --git a/KugleFirmware/Libraries/Modules/Estimators/VelocityEKF/MATLABCoder/SteadyStateAcceleration.cpp b/KugleFirmware/Libraries/Modules/Estimators/VelocityEKF/MATLABCoder/SteadyStateAcceleration.cpp
new file mode 100644
index 0000000..ea7d95c
--- /dev/null
+++ b/KugleFirmware/Libraries/Modules/Estimators/VelocityEKF/MATLABCoder/SteadyStateAcceleration.cpp
@@ -0,0 +1,2931 @@
+//
+// Academic License - for use in teaching, academic research, and meeting
+// course requirements at degree granting institutions only.  Not for
+// government, commercial, or other organizational use.
+// File: SteadyStateAcceleration.cpp
+//
+// MATLAB Coder version            : 4.0
+// C/C++ source code generated on  : 19-Nov-2018 11:57:59
+//
+
+// Include Files
+#include "rt_nonfinite.h"
+#include "VelocityEstimator.h"
+#include "SteadyStateAcceleration.h"
+
+// Function Definitions
+
+//
+// function acceleration = SteadyStateAcceleration(COM_X,COM_Y,COM_Z,Jk,Jw,Mb,Mk,dx,dy,g,q1,q2,q3,q4,rk,rw)
+// STEADYSTATEACCELERATION
+//     ACCELERATION = STEADYSTATEACCELERATION(COM_X,COM_Y,COM_Z,JK,JW,MB,MK,DX,DY,G,Q1,Q2,Q3,Q4,RK,RW)
+// Arguments    : float COM_X
+//                float COM_Y
+//                float COM_Z
+//                float Jk
+//                float Jw
+//                float Mb
+//                float Mk
+//                float dx
+//                float dy
+//                float g
+//                float q1
+//                float q2
+//                float q3
+//                float q4
+//                float rk
+//                float rw
+//                float acceleration[2]
+// Return Type  : void
+//
+__attribute__((optimize("O3"))) void SteadyStateAcceleration(float COM_X, float COM_Y, float COM_Z, float Jk,
+  float Jw, float Mb, float Mk, float dx, float dy, float g, float q1, float q2,
+  float q3, float q4, float rk, float rw, float acceleration[2])
+{
+  float t2;
+  float t3;
+  float t4;
+  float t5;
+  float t6;
+  float t7;
+  float t8;
+  float t9;
+  float t10;
+  float t11;
+  float t12;
+  float t13;
+  float t14;
+  float t15;
+  float t16;
+  float t17;
+  float t18;
+  float t19;
+  float t20;
+  float t21;
+  float t22;
+  float t23;
+  float t24;
+  float f11;
+  float f12;
+  float f13;
+  float f14;
+  float f15;
+  float f16;
+  float f17;
+  float f18;
+  float f19;
+  float f20;
+  float f21;
+  float f22;
+  float f23;
+  float f24;
+  float f25;
+  float f26;
+  float f27;
+  float f28;
+  float f29;
+  float f30;
+  float f31;
+  float f32;
+  float t476;
+
+  //     This function was generated by the Symbolic Math Toolbox version 8.1.
+  //     16-Nov-2018 12:04:12
+  // 'SteadyStateAcceleration:8' t2 = q1.^2;
+  t2 = q1 * q1;
+
+  // 'SteadyStateAcceleration:9' t3 = q3.^2;
+  t3 = q3 * q3;
+
+  // 'SteadyStateAcceleration:10' t4 = t3.^2;
+  t4 = t3 * t3;
+
+  // 'SteadyStateAcceleration:11' t5 = Jw.^2;
+  t5 = Jw * Jw;
+
+  // 'SteadyStateAcceleration:12' t6 = dy.^2;
+  t6 = dy * dy;
+
+  // 'SteadyStateAcceleration:13' t7 = t2.^2;
+  t7 = t2 * t2;
+
+  // 'SteadyStateAcceleration:14' t8 = rk.^2;
+  t8 = rk * rk;
+
+  // 'SteadyStateAcceleration:15' t9 = q2.^2;
+  t9 = q2 * q2;
+
+  // 'SteadyStateAcceleration:16' t10 = q4.^2;
+  t10 = q4 * q4;
+
+  // 'SteadyStateAcceleration:17' t11 = t10.^2;
+  t11 = t10 * t10;
+
+  // 'SteadyStateAcceleration:18' t12 = t9.^2;
+  t12 = t9 * t9;
+
+  // 'SteadyStateAcceleration:19' t13 = t4.^2;
+  t13 = t4 * t4;
+
+  // 'SteadyStateAcceleration:20' t14 = t7.^2;
+  t14 = t7 * t7;
+
+  // 'SteadyStateAcceleration:21' t15 = t11.^2;
+  t15 = t11 * t11;
+
+  // 'SteadyStateAcceleration:22' t16 = t12.^2;
+  t16 = t12 * t12;
+
+  // 'SteadyStateAcceleration:23' t17 = dx.^2;
+  t17 = dx * dx;
+
+  // 'SteadyStateAcceleration:24' t18 = rw.^2;
+  t18 = rw * rw;
+
+  // 'SteadyStateAcceleration:25' t19 = Mb.^2;
+  t19 = Mb * Mb;
+
+  // 'SteadyStateAcceleration:26' t20 = t18.^2;
+  t20 = t18 * t18;
+
+  // 'SteadyStateAcceleration:27' t21 = COM_X.^2;
+  t21 = COM_X * COM_X;
+
+  // 'SteadyStateAcceleration:28' t22 = COM_Z.^2;
+  t22 = COM_Z * COM_Z;
+
+  // 'SteadyStateAcceleration:29' t23 = COM_Y.^2;
+  t23 = COM_Y * COM_Y;
+
+  // 'SteadyStateAcceleration:30' t24 = t8.^2;
+  t24 = t8 * t8;
+
+  // 'SteadyStateAcceleration:31' t25 = Jk.^2;
+  // 'SteadyStateAcceleration:32' t26 = t20.*t25.*1.6e1;
+  // 'SteadyStateAcceleration:33' t27 = t5.*t14.*t24.*9.0;
+  // 'SteadyStateAcceleration:34' t28 = t5.*t16.*t24.*9.0;
+  // 'SteadyStateAcceleration:35' t29 = t5.*t13.*t24.*9.0;
+  // 'SteadyStateAcceleration:36' t30 = t5.*t15.*t24.*9.0;
+  // 'SteadyStateAcceleration:37' t31 = t5.*t7.*t14.*t24.*9.0;
+  // 'SteadyStateAcceleration:38' t32 = t5.*t12.*t16.*t24.*9.0;
+  // 'SteadyStateAcceleration:39' t33 = t4.*t5.*t13.*t24.*9.0;
+  // 'SteadyStateAcceleration:40' t34 = t5.*t11.*t15.*t24.*9.0;
+  // 'SteadyStateAcceleration:41' t35 = t19.*t20.*t24.*1.6e1;
+  // 'SteadyStateAcceleration:42' t36 = Mk.^2;
+  // 'SteadyStateAcceleration:43' t37 = t20.*t24.*t36.*1.6e1;
+  // 'SteadyStateAcceleration:44' t38 = t2.*t5.*t9.*t12.*t24.*7.2e1;
+  // 'SteadyStateAcceleration:45' t39 = t5.*t7.*t12.*t24.*1.26e2;
+  // 'SteadyStateAcceleration:46' t40 = t2.*t5.*t7.*t9.*t24.*7.2e1;
+  // 'SteadyStateAcceleration:47' t41 = t2.*t3.*t4.*t5.*t24.*7.2e1;
+  // 'SteadyStateAcceleration:48' t42 = t4.*t5.*t7.*t24.*1.26e2;
+  // 'SteadyStateAcceleration:49' t43 = t2.*t3.*t5.*t7.*t24.*7.2e1;
+  // 'SteadyStateAcceleration:50' t44 = t2.*t5.*t10.*t11.*t24.*3.6e1;
+  // 'SteadyStateAcceleration:51' t45 = t3.*t4.*t5.*t9.*t24.*3.6e1;
+  // 'SteadyStateAcceleration:52' t46 = t5.*t7.*t11.*t24.*5.4e1;
+  // 'SteadyStateAcceleration:53' t47 = t4.*t5.*t12.*t24.*5.4e1;
+  // 'SteadyStateAcceleration:54' t48 = t2.*t5.*t7.*t10.*t24.*3.6e1;
+  // 'SteadyStateAcceleration:55' t49 = t3.*t5.*t9.*t12.*t24.*3.6e1;
+  // 'SteadyStateAcceleration:56' t50 = t5.*t9.*t10.*t11.*t24.*7.2e1;
+  // 'SteadyStateAcceleration:57' t51 = t5.*t11.*t12.*t24.*1.26e2;
+  // 'SteadyStateAcceleration:58' t52 = t5.*t9.*t10.*t12.*t24.*7.2e1;
+  // 'SteadyStateAcceleration:59' t53 = t2.*t5.*t9.*t16.*t24.*9.0e1;
+  // 'SteadyStateAcceleration:60' t54 = t5.*t7.*t16.*t24.*2.79e2;
+  // 'SteadyStateAcceleration:61' t55 = t3.*t5.*t10.*t11.*t24.*7.2e1;
+  // 'SteadyStateAcceleration:62' t56 = t2.*t5.*t7.*t9.*t12.*t24.*3.96e2;
+  // 'SteadyStateAcceleration:63' t57 = t4.*t5.*t11.*t24.*1.26e2;
+  // 'SteadyStateAcceleration:64' t58 = t5.*t12.*t14.*t24.*2.79e2;
+  // 'SteadyStateAcceleration:65' t59 = t3.*t4.*t5.*t10.*t24.*7.2e1;
+  // 'SteadyStateAcceleration:66' t60 = t2.*t5.*t9.*t14.*t24.*9.0e1;
+  // 'SteadyStateAcceleration:67' t61 = t2.*t3.*t5.*t13.*t24.*9.0e1;
+  // 'SteadyStateAcceleration:68' t62 = t5.*t7.*t13.*t24.*2.79e2;
+  // 'SteadyStateAcceleration:69' t63 = t2.*t3.*t4.*t5.*t7.*t24.*3.96e2;
+  // 'SteadyStateAcceleration:70' t64 = t4.*t5.*t14.*t24.*2.79e2;
+  // 'SteadyStateAcceleration:71' t65 = t2.*t3.*t5.*t14.*t24.*9.0e1;
+  // 'SteadyStateAcceleration:72' t66 = t2.*t5.*t10.*t15.*t24.*5.4e1;
+  // 'SteadyStateAcceleration:73' t67 = t3.*t5.*t9.*t13.*t24.*5.4e1;
+  // 'SteadyStateAcceleration:74' t68 = t5.*t7.*t15.*t24.*1.35e2;
+  // 'SteadyStateAcceleration:75' t69 = t5.*t12.*t13.*t24.*1.35e2;
+  // 'SteadyStateAcceleration:76' t70 = t2.*t5.*t7.*t10.*t11.*t24.*1.8e2;
+  // 'SteadyStateAcceleration:77' t71 = t3.*t4.*t5.*t9.*t12.*t24.*1.8e2;
+  // 'SteadyStateAcceleration:78' t72 = t5.*t11.*t14.*t24.*1.35e2;
+  // 'SteadyStateAcceleration:79' t73 = t4.*t5.*t16.*t24.*1.35e2;
+  // 'SteadyStateAcceleration:80' t74 = t2.*t5.*t10.*t14.*t24.*5.4e1;
+  // 'SteadyStateAcceleration:81' t75 = t3.*t5.*t9.*t16.*t24.*5.4e1;
+  // 'SteadyStateAcceleration:82' t76 = t5.*t9.*t10.*t15.*t24.*9.0e1;
+  // 'SteadyStateAcceleration:83' t77 = t5.*t12.*t15.*t24.*2.79e2;
+  // 'SteadyStateAcceleration:84' t78 = t5.*t9.*t10.*t11.*t12.*t24.*3.96e2;
+  // 'SteadyStateAcceleration:85' t79 = t5.*t11.*t16.*t24.*2.79e2;
+  // 'SteadyStateAcceleration:86' t80 = t5.*t9.*t10.*t16.*t24.*9.0e1;
+  // 'SteadyStateAcceleration:87' t81 = t3.*t5.*t10.*t15.*t24.*9.0e1;
+  // 'SteadyStateAcceleration:88' t82 = t4.*t5.*t15.*t24.*2.79e2;
+  // 'SteadyStateAcceleration:89' t83 = t3.*t4.*t5.*t10.*t11.*t24.*3.96e2;
+  // 'SteadyStateAcceleration:90' t84 = t5.*t11.*t13.*t24.*2.79e2;
+  // 'SteadyStateAcceleration:91' t85 = t3.*t5.*t10.*t13.*t24.*9.0e1;
+  // 'SteadyStateAcceleration:92' t86 = Jk.*Mb.*t8.*t20.*3.2e1;
+  // 'SteadyStateAcceleration:93' t87 = Jk.*Mk.*t8.*t20.*3.2e1;
+  // 'SteadyStateAcceleration:94' t88 = Mb.*Mk.*t20.*t24.*3.2e1;
+  // 'SteadyStateAcceleration:95' t89 = t8.*t14.*t19.*t20.*t22.*1.6e1;
+  // 'SteadyStateAcceleration:96' t90 = t8.*t16.*t19.*t20.*t22.*1.6e1;
+  // 'SteadyStateAcceleration:97' t91 = t8.*t13.*t19.*t20.*t22.*1.6e1;
+  // 'SteadyStateAcceleration:98' t92 = t8.*t15.*t19.*t20.*t22.*1.6e1;
+  // 'SteadyStateAcceleration:99' t93 = t2.*t4.*t5.*t9.*t24.*2.16e2;
+  // 'SteadyStateAcceleration:100' t94 = t2.*t3.*t5.*t12.*t24.*2.16e2;
+  // 'SteadyStateAcceleration:101' t95 = t3.*t5.*t7.*t9.*t24.*2.52e2;
+  // 'SteadyStateAcceleration:102' t96 = t2.*t5.*t9.*t11.*t24.*2.16e2;
+  // 'SteadyStateAcceleration:103' t97 = t2.*t5.*t10.*t12.*t24.*2.52e2;
+  // 'SteadyStateAcceleration:104' t98 = t5.*t7.*t9.*t10.*t24.*2.16e2;
+  // 'SteadyStateAcceleration:105' t99 = t2.*t3.*t5.*t11.*t24.*2.16e2;
+  // 'SteadyStateAcceleration:106' t100 = t2.*t4.*t5.*t10.*t24.*2.52e2;
+  // 'SteadyStateAcceleration:107' t101 = t3.*t5.*t7.*t10.*t24.*2.16e2;
+  // 'SteadyStateAcceleration:108' t102 = t3.*t5.*t9.*t11.*t24.*2.52e2;
+  // 'SteadyStateAcceleration:109' t103 = t4.*t5.*t9.*t10.*t24.*2.16e2;
+  // 'SteadyStateAcceleration:110' t104 = t3.*t5.*t10.*t12.*t24.*2.16e2;
+  // 'SteadyStateAcceleration:111' t105 = t2.*t5.*t9.*t13.*t24.*4.5e2;
+  // 'SteadyStateAcceleration:112' t106 = t2.*t3.*t4.*t5.*t12.*t24.*9.0e2;
+  // 'SteadyStateAcceleration:113' t107 = t2.*t4.*t5.*t9.*t12.*t24.*9.0e2;
+  // 'SteadyStateAcceleration:114' t108 = t2.*t3.*t5.*t16.*t24.*4.5e2;
+  // 'SteadyStateAcceleration:115' t109 = t3.*t4.*t5.*t7.*t9.*t24.*1.116e3;
+  // 'SteadyStateAcceleration:116' t110 = t4.*t5.*t7.*t12.*t24.*1.674e3;
+  // 'SteadyStateAcceleration:117' t111 = t3.*t5.*t7.*t9.*t12.*t24.*1.116e3;
+  // 'SteadyStateAcceleration:118' t112 = t2.*t4.*t5.*t7.*t9.*t24.*1.188e3;
+  // 'SteadyStateAcceleration:119' t113 = t2.*t3.*t5.*t7.*t12.*t24.*1.188e3;
+  // 'SteadyStateAcceleration:120' t114 = t3.*t5.*t9.*t14.*t24.*5.58e2;
+  // 'SteadyStateAcceleration:121' t115 = t2.*t5.*t9.*t15.*t24.*4.5e2;
+  // 'SteadyStateAcceleration:122' t116 = t2.*t5.*t10.*t11.*t12.*t24.*1.116e3;
+  // 'SteadyStateAcceleration:123' t117 = t2.*t5.*t9.*t11.*t12.*t24.*1.188e3;
+  // 'SteadyStateAcceleration:124' t118 = t2.*t5.*t10.*t16.*t24.*5.58e2;
+  // 'SteadyStateAcceleration:125' t119 = t5.*t7.*t9.*t10.*t11.*t24.*9.0e2;
+  // 'SteadyStateAcceleration:126' t120 = t5.*t7.*t11.*t12.*t24.*1.674e3;
+  // 'SteadyStateAcceleration:127' t121 = t5.*t7.*t9.*t10.*t12.*t24.*1.188e3;
+  // 'SteadyStateAcceleration:128' t122 = t2.*t5.*t7.*t9.*t11.*t24.*9.0e2;
+  // 'SteadyStateAcceleration:129' t123 = t2.*t5.*t7.*t10.*t12.*t24.*1.116e3;
+  // 'SteadyStateAcceleration:130' t124 = t5.*t9.*t10.*t14.*t24.*4.5e2;
+  // 'SteadyStateAcceleration:131' t125 = t2.*t3.*t5.*t15.*t24.*4.5e2;
+  // 'SteadyStateAcceleration:132' t126 = t2.*t4.*t5.*t10.*t11.*t24.*1.116e3;
+  // 'SteadyStateAcceleration:133' t127 = t2.*t3.*t4.*t5.*t11.*t24.*1.188e3;
+  // 'SteadyStateAcceleration:134' t128 = t2.*t5.*t10.*t13.*t24.*5.58e2;
+  // 'SteadyStateAcceleration:135' t129 = t3.*t5.*t7.*t10.*t11.*t24.*9.0e2;
+  // 'SteadyStateAcceleration:136' t130 = t4.*t5.*t7.*t11.*t24.*1.674e3;
+  // 'SteadyStateAcceleration:137' t131 = t3.*t4.*t5.*t7.*t10.*t24.*1.188e3;
+  // 'SteadyStateAcceleration:138' t132 = t2.*t3.*t5.*t7.*t11.*t24.*9.0e2;
+  // 'SteadyStateAcceleration:139' t133 = t2.*t4.*t5.*t7.*t10.*t24.*1.116e3;
+  // 'SteadyStateAcceleration:140' t134 = t3.*t5.*t10.*t14.*t24.*4.5e2;
+  // 'SteadyStateAcceleration:141' t135 = t3.*t5.*t9.*t15.*t24.*5.58e2;
+  // 'SteadyStateAcceleration:142' t136 = t4.*t5.*t9.*t10.*t11.*t24.*1.188e3;
+  // 'SteadyStateAcceleration:143' t137 = t3.*t4.*t5.*t9.*t11.*t24.*1.116e3;
+  // 'SteadyStateAcceleration:144' t138 = t5.*t9.*t10.*t13.*t24.*4.5e2;
+  // 'SteadyStateAcceleration:145' t139 = t3.*t5.*t10.*t11.*t12.*t24.*1.188e3;
+  // 'SteadyStateAcceleration:146' t140 = t4.*t5.*t11.*t12.*t24.*1.674e3;
+  // 'SteadyStateAcceleration:147' t141 = t3.*t4.*t5.*t10.*t12.*t24.*9.0e2;
+  // 'SteadyStateAcceleration:148' t142 = t3.*t5.*t9.*t11.*t12.*t24.*1.116e3;
+  // 'SteadyStateAcceleration:149' t143 = t4.*t5.*t9.*t10.*t12.*t24.*9.0e2;
+  // 'SteadyStateAcceleration:150' t144 = t3.*t5.*t10.*t16.*t24.*4.5e2;
+  // 'SteadyStateAcceleration:151' t145 = Jk.*Jw.*t7.*t8.*t18.*2.4e1;
+  // 'SteadyStateAcceleration:152' t146 = Jk.*Jw.*t8.*t12.*t18.*2.4e1;
+  // 'SteadyStateAcceleration:153' t147 = Jk.*Jw.*t4.*t8.*t18.*2.4e1;
+  // 'SteadyStateAcceleration:154' t148 = Jk.*Jw.*t8.*t11.*t18.*2.4e1;
+  // 'SteadyStateAcceleration:155' t149 = Jw.*Mb.*t7.*t18.*t24.*2.4e1;
+  // 'SteadyStateAcceleration:156' t150 = Jw.*Mb.*t12.*t18.*t24.*2.4e1;
+  // 'SteadyStateAcceleration:157' t151 = Jw.*Mb.*t4.*t18.*t24.*2.4e1;
+  // 'SteadyStateAcceleration:158' t152 = Jw.*Mb.*t11.*t18.*t24.*2.4e1;
+  // 'SteadyStateAcceleration:159' t153 = Jw.*Mk.*t7.*t18.*t24.*2.4e1;
+  // 'SteadyStateAcceleration:160' t154 = Jw.*Mk.*t12.*t18.*t24.*2.4e1;
+  // 'SteadyStateAcceleration:161' t155 = Jw.*Mk.*t4.*t18.*t24.*2.4e1;
+  // 'SteadyStateAcceleration:162' t156 = Jw.*Mk.*t11.*t18.*t24.*2.4e1;
+  // 'SteadyStateAcceleration:163' t157 = COM_Z.*rk.*t7.*t8.*t19.*t20.*3.2e1;
+  // 'SteadyStateAcceleration:164' t158 = COM_Z.*rk.*t8.*t11.*t19.*t20.*3.2e1;
+  // 'SteadyStateAcceleration:165' t159 = t2.*t3.*t5.*t9.*t10.*t24.*5.04e2;
+  // 'SteadyStateAcceleration:166' t160 = t2.*t3.*t5.*t9.*t10.*t11.*t24.*2.232e3; 
+  // 'SteadyStateAcceleration:167' t161 = t2.*t4.*t5.*t9.*t11.*t24.*3.564e3;
+  // 'SteadyStateAcceleration:168' t162 = t2.*t3.*t4.*t5.*t9.*t10.*t24.*2.232e3; 
+  // 'SteadyStateAcceleration:169' t163 = t2.*t3.*t5.*t11.*t12.*t24.*3.564e3;
+  // 'SteadyStateAcceleration:170' t164 = t2.*t4.*t5.*t10.*t12.*t24.*3.348e3;
+  // 'SteadyStateAcceleration:171' t165 = t2.*t3.*t5.*t9.*t10.*t12.*t24.*2.232e3; 
+  // 'SteadyStateAcceleration:172' t166 = t3.*t5.*t7.*t9.*t11.*t24.*3.348e3;
+  // 'SteadyStateAcceleration:173' t167 = t4.*t5.*t7.*t9.*t10.*t24.*3.564e3;
+  // 'SteadyStateAcceleration:174' t168 = t3.*t5.*t7.*t10.*t12.*t24.*3.564e3;
+  // 'SteadyStateAcceleration:175' t169 = t2.*t3.*t5.*t7.*t9.*t10.*t24.*2.232e3; 
+  // 'SteadyStateAcceleration:176' t170 = COM_Y.*q1.*q2.*rk.*t8.*t9.*t19.*t20.*6.4e1; 
+  // 'SteadyStateAcceleration:177' t171 = COM_Y.*q1.*q2.*rk.*t2.*t8.*t19.*t20.*6.4e1; 
+  // 'SteadyStateAcceleration:178' t172 = COM_X.*q2.*q4.*rk.*t8.*t10.*t19.*t20.*6.4e1; 
+  // 'SteadyStateAcceleration:179' t173 = COM_X.*q2.*q4.*rk.*t8.*t9.*t19.*t20.*6.4e1; 
+  // 'SteadyStateAcceleration:180' t174 = COM_Y.*q3.*q4.*rk.*t8.*t10.*t19.*t20.*6.4e1; 
+  // 'SteadyStateAcceleration:181' t175 = COM_Y.*q3.*q4.*rk.*t3.*t8.*t19.*t20.*6.4e1; 
+  // 'SteadyStateAcceleration:182' t176 = Jk.*Jw.*t2.*t8.*t9.*t18.*9.6e1;
+  // 'SteadyStateAcceleration:183' t177 = Jk.*Jw.*t2.*t3.*t8.*t18.*9.6e1;
+  // 'SteadyStateAcceleration:184' t178 = Jk.*Jw.*t2.*t8.*t10.*t18.*4.8e1;
+  // 'SteadyStateAcceleration:185' t179 = Jk.*Jw.*t3.*t8.*t9.*t18.*4.8e1;
+  // 'SteadyStateAcceleration:186' t180 = Jk.*Jw.*t8.*t9.*t10.*t18.*9.6e1;
+  // 'SteadyStateAcceleration:187' t181 = Jk.*Jw.*t3.*t8.*t10.*t18.*9.6e1;
+  // 'SteadyStateAcceleration:188' t182 = Jw.*Mb.*t2.*t9.*t18.*t24.*9.6e1;
+  // 'SteadyStateAcceleration:189' t183 = Jw.*Mb.*t2.*t3.*t18.*t24.*9.6e1;
+  // 'SteadyStateAcceleration:190' t184 = Jw.*Mb.*t2.*t10.*t18.*t24.*4.8e1;
+  // 'SteadyStateAcceleration:191' t185 = Jw.*Mb.*t3.*t9.*t18.*t24.*4.8e1;
+  // 'SteadyStateAcceleration:192' t186 = Jw.*Mb.*t9.*t10.*t18.*t24.*9.6e1;
+  // 'SteadyStateAcceleration:193' t187 = Jw.*Mb.*t3.*t10.*t18.*t24.*9.6e1;
+  // 'SteadyStateAcceleration:194' t188 = Jw.*Mk.*t2.*t9.*t18.*t24.*9.6e1;
+  // 'SteadyStateAcceleration:195' t189 = Jw.*Mk.*t2.*t3.*t18.*t24.*9.6e1;
+  // 'SteadyStateAcceleration:196' t190 = Jw.*Mk.*t2.*t10.*t18.*t24.*4.8e1;
+  // 'SteadyStateAcceleration:197' t191 = Jw.*Mk.*t3.*t9.*t18.*t24.*4.8e1;
+  // 'SteadyStateAcceleration:198' t192 = Jw.*Mk.*t9.*t10.*t18.*t24.*9.6e1;
+  // 'SteadyStateAcceleration:199' t193 = Jw.*Mk.*t3.*t10.*t18.*t24.*9.6e1;
+  // 'SteadyStateAcceleration:200' t194 = COM_Z.*Jk.*Mb.*rk.*t7.*t20.*3.2e1;
+  // 'SteadyStateAcceleration:201' t195 = COM_Z.*Jk.*Mb.*rk.*t11.*t20.*3.2e1;
+  // 'SteadyStateAcceleration:202' t196 = COM_Z.*rk.*t2.*t8.*t10.*t19.*t20.*6.4e1; 
+  // 'SteadyStateAcceleration:203' t197 = COM_Z.*Jw.*Mb.*rk.*t8.*t14.*t18.*2.4e1; 
+  // 'SteadyStateAcceleration:204' t198 = COM_Z.*Jw.*Mb.*rk.*t8.*t9.*t16.*t18.*2.4e1; 
+  // 'SteadyStateAcceleration:205' t199 = COM_Z.*Jw.*Mb.*rk.*t8.*t15.*t18.*2.4e1; 
+  // 'SteadyStateAcceleration:206' t200 = COM_Z.*Jw.*Mb.*rk.*t3.*t8.*t13.*t18.*2.4e1; 
+  // 'SteadyStateAcceleration:207' t201 = COM_Z.*Mb.*Mk.*rk.*t7.*t8.*t20.*3.2e1; 
+  // 'SteadyStateAcceleration:208' t202 = COM_Z.*Mb.*Mk.*rk.*t8.*t11.*t20.*3.2e1; 
+  // 'SteadyStateAcceleration:209' t203 = t2.*t3.*t4.*t8.*t19.*t20.*t21.*6.4e1;
+  // 'SteadyStateAcceleration:210' t204 = t4.*t7.*t8.*t19.*t20.*t21.*1.28e2;
+  // 'SteadyStateAcceleration:211' t205 = t2.*t3.*t7.*t8.*t19.*t20.*t21.*6.4e1;
+  // 'SteadyStateAcceleration:212' t206 = t2.*t8.*t9.*t12.*t19.*t20.*t23.*6.4e1; 
+  // 'SteadyStateAcceleration:213' t207 = t7.*t8.*t12.*t19.*t20.*t23.*1.28e2;
+  // 'SteadyStateAcceleration:214' t208 = t2.*t7.*t8.*t9.*t19.*t20.*t23.*6.4e1;
+  // 'SteadyStateAcceleration:215' t209 = t8.*t9.*t10.*t11.*t19.*t20.*t21.*6.4e1; 
+  // 'SteadyStateAcceleration:216' t210 = t8.*t11.*t12.*t19.*t20.*t21.*1.28e2;
+  // 'SteadyStateAcceleration:217' t211 = t8.*t9.*t10.*t12.*t19.*t20.*t21.*6.4e1; 
+  // 'SteadyStateAcceleration:218' t212 = t2.*t8.*t10.*t11.*t19.*t20.*t22.*6.4e1; 
+  // 'SteadyStateAcceleration:219' t213 = t3.*t4.*t8.*t9.*t19.*t20.*t22.*6.4e1;
+  // 'SteadyStateAcceleration:220' t214 = t7.*t8.*t11.*t19.*t20.*t22.*9.6e1;
+  // 'SteadyStateAcceleration:221' t215 = t4.*t8.*t12.*t19.*t20.*t22.*9.6e1;
+  // 'SteadyStateAcceleration:222' t216 = t2.*t7.*t8.*t10.*t19.*t20.*t22.*6.4e1; 
+  // 'SteadyStateAcceleration:223' t217 = t3.*t8.*t9.*t12.*t19.*t20.*t22.*6.4e1; 
+  // 'SteadyStateAcceleration:224' t218 = t3.*t8.*t10.*t11.*t19.*t20.*t23.*6.4e1; 
+  // 'SteadyStateAcceleration:225' t219 = t4.*t8.*t11.*t19.*t20.*t23.*1.28e2;
+  // 'SteadyStateAcceleration:226' t220 = t3.*t4.*t8.*t10.*t19.*t20.*t23.*6.4e1; 
+  // 'SteadyStateAcceleration:227' t221 = COM_Z.*Jk.*Mb.*rk.*t2.*t10.*t20.*6.4e1; 
+  // 'SteadyStateAcceleration:228' t222 = COM_Y.*Jw.*Mb.*q1.*q2.*rk.*t8.*t9.*t12.*t18.*4.8e1; 
+  // 'SteadyStateAcceleration:229' t223 = COM_Y.*Jw.*Mb.*q1.*q2.*rk.*t2.*t7.*t8.*t18.*4.8e1; 
+  // 'SteadyStateAcceleration:230' t224 = COM_X.*Jw.*Mb.*q1.*q3.*rk.*t8.*t13.*t18.*4.8e1; 
+  // 'SteadyStateAcceleration:231' t225 = COM_X.*Jw.*Mb.*q2.*q4.*rk.*t8.*t10.*t11.*t18.*4.8e1; 
+  // 'SteadyStateAcceleration:232' t226 = COM_X.*Jw.*Mb.*q2.*q4.*rk.*t8.*t9.*t12.*t18.*4.8e1; 
+  // 'SteadyStateAcceleration:233' t227 = COM_X.*Jw.*Mb.*q1.*q3.*rk.*t8.*t14.*t18.*4.8e1; 
+  // 'SteadyStateAcceleration:234' t228 = COM_Y.*Jw.*Mb.*q3.*q4.*rk.*t8.*t10.*t11.*t18.*4.8e1; 
+  // 'SteadyStateAcceleration:235' t229 = COM_Y.*Jw.*Mb.*q3.*q4.*rk.*t3.*t4.*t8.*t18.*4.8e1; 
+  // 'SteadyStateAcceleration:236' t230 = COM_Y.*Mb.*Mk.*q1.*q2.*rk.*t8.*t9.*t20.*6.4e1; 
+  // 'SteadyStateAcceleration:237' t231 = COM_Y.*Mb.*Mk.*q1.*q2.*rk.*t2.*t8.*t20.*6.4e1; 
+  // 'SteadyStateAcceleration:238' t232 = COM_X.*Mb.*Mk.*q2.*q4.*rk.*t8.*t10.*t20.*6.4e1; 
+  // 'SteadyStateAcceleration:239' t233 = COM_X.*Mb.*Mk.*q2.*q4.*rk.*t8.*t9.*t20.*6.4e1; 
+  // 'SteadyStateAcceleration:240' t234 = COM_Y.*Mb.*Mk.*q3.*q4.*rk.*t8.*t10.*t20.*6.4e1; 
+  // 'SteadyStateAcceleration:241' t235 = COM_Y.*Mb.*Mk.*q3.*q4.*rk.*t3.*t8.*t20.*6.4e1; 
+  // 'SteadyStateAcceleration:242' t236 = COM_X.*COM_Z.*q1.*q3.*t3.*t4.*t8.*t19.*t20.*6.4e1; 
+  // 'SteadyStateAcceleration:243' t237 = COM_Y.*COM_Z.*q1.*q2.*t2.*t7.*t8.*t19.*t20.*6.4e1; 
+  // 'SteadyStateAcceleration:244' t238 = COM_X.*COM_Z.*q2.*q4.*t8.*t10.*t11.*t19.*t20.*6.4e1; 
+  // 'SteadyStateAcceleration:245' t239 = COM_Y.*COM_Z.*q3.*q4.*t8.*t10.*t11.*t19.*t20.*6.4e1; 
+  // 'SteadyStateAcceleration:246' t240 = COM_Y.*Jw.*Mb.*q1.*q2.*rk.*t2.*t8.*t12.*t18.*2.4e2; 
+  // 'SteadyStateAcceleration:247' t241 = COM_Y.*Jw.*Mb.*q1.*q2.*rk.*t7.*t8.*t9.*t18.*2.4e2; 
+  // 'SteadyStateAcceleration:248' t242 = COM_Z.*Jw.*Mb.*rk.*t2.*t7.*t8.*t9.*t18.*9.6e1; 
+  // 'SteadyStateAcceleration:249' t243 = COM_X.*Jw.*Mb.*q1.*q3.*rk.*t2.*t3.*t4.*t8.*t18.*2.88e2; 
+  // 'SteadyStateAcceleration:250' t244 = COM_X.*Jw.*Mb.*q2.*q4.*rk.*t8.*t9.*t11.*t18.*2.4e2; 
+  // 'SteadyStateAcceleration:251' t245 = COM_X.*Jw.*Mb.*q1.*q3.*rk.*t4.*t7.*t8.*t18.*4.8e2; 
+  // 'SteadyStateAcceleration:252' t246 = COM_X.*Jw.*Mb.*q2.*q4.*rk.*t8.*t10.*t12.*t18.*2.4e2; 
+  // 'SteadyStateAcceleration:253' t247 = COM_X.*Jw.*Mb.*q1.*q3.*rk.*t2.*t3.*t7.*t8.*t18.*2.88e2; 
+  // 'SteadyStateAcceleration:254' t248 = COM_Z.*Jw.*Mb.*rk.*t2.*t3.*t7.*t8.*t18.*9.6e1; 
+  // 'SteadyStateAcceleration:255' t249 = COM_Z.*Jw.*Mb.*rk.*t2.*t8.*t16.*t18.*1.2e2; 
+  // 'SteadyStateAcceleration:256' t250 = COM_Z.*Jw.*Mb.*rk.*t2.*t8.*t10.*t11.*t18.*9.6e1; 
+  // 'SteadyStateAcceleration:257' t251 = COM_Z.*Jw.*Mb.*rk.*t7.*t8.*t9.*t12.*t18.*9.6e1; 
+  // 'SteadyStateAcceleration:258' t252 = COM_Z.*Jw.*Mb.*rk.*t7.*t8.*t11.*t18.*1.44e2; 
+  // 'SteadyStateAcceleration:259' t253 = COM_Z.*Jw.*Mb.*rk.*t2.*t7.*t8.*t10.*t18.*9.6e1; 
+  // 'SteadyStateAcceleration:260' t254 = COM_Y.*Jw.*Mb.*q3.*q4.*rk.*t3.*t8.*t11.*t18.*2.4e2; 
+  // 'SteadyStateAcceleration:261' t255 = COM_Y.*Jw.*Mb.*q3.*q4.*rk.*t4.*t8.*t10.*t18.*2.4e2; 
+  // 'SteadyStateAcceleration:262' t256 = COM_Z.*Jw.*Mb.*rk.*t2.*t8.*t13.*t18.*1.2e2; 
+  // 'SteadyStateAcceleration:263' t257 = COM_Z.*Jw.*Mb.*rk.*t8.*t9.*t10.*t11.*t18.*9.6e1; 
+  // 'SteadyStateAcceleration:264' t258 = COM_Z.*Jw.*Mb.*rk.*t3.*t4.*t7.*t8.*t18.*9.6e1; 
+  // 'SteadyStateAcceleration:265' t259 = COM_Z.*Jw.*Mb.*rk.*t8.*t9.*t13.*t18.*1.2e2; 
+  // 'SteadyStateAcceleration:266' t260 = COM_Z.*Jw.*Mb.*rk.*t3.*t8.*t10.*t11.*t18.*9.6e1; 
+  // 'SteadyStateAcceleration:267' t261 = COM_Z.*Jw.*Mb.*rk.*t3.*t4.*t8.*t12.*t18.*2.4e2; 
+  // 'SteadyStateAcceleration:268' t262 = COM_Z.*Jw.*Mb.*rk.*t4.*t8.*t9.*t12.*t18.*2.4e2; 
+  // 'SteadyStateAcceleration:269' t263 = COM_Z.*Jw.*Mb.*rk.*t3.*t8.*t16.*t18.*1.2e2; 
+  // 'SteadyStateAcceleration:270' t264 = COM_Z.*Jw.*Mb.*rk.*t8.*t9.*t11.*t12.*t18.*9.6e1; 
+  // 'SteadyStateAcceleration:271' t265 = COM_Z.*Jw.*Mb.*rk.*t8.*t10.*t16.*t18.*1.2e2; 
+  // 'SteadyStateAcceleration:272' t266 = COM_Z.*Jw.*Mb.*rk.*t3.*t4.*t8.*t11.*t18.*9.6e1; 
+  // 'SteadyStateAcceleration:273' t267 = COM_Z.*Jw.*Mb.*rk.*t8.*t10.*t13.*t18.*1.2e2; 
+  // 'SteadyStateAcceleration:274' t268 = COM_Z.*Mb.*Mk.*rk.*t2.*t8.*t10.*t20.*6.4e1; 
+  // 'SteadyStateAcceleration:275' t269 = COM_X.*q2.*q4.*rk.*t2.*t8.*t19.*t20.*6.4e1; 
+  // 'SteadyStateAcceleration:276' t270 = COM_Y.*q1.*q2.*rk.*t3.*t8.*t19.*t20.*6.4e1; 
+  // 'SteadyStateAcceleration:277' t271 = COM_Y.*q1.*q2.*rk.*t8.*t10.*t19.*t20.*6.4e1; 
+  // 'SteadyStateAcceleration:278' t272 = COM_X.*q2.*q4.*rk.*t3.*t8.*t19.*t20.*6.4e1; 
+  // 'SteadyStateAcceleration:279' t273 = COM_Y.*q3.*q4.*rk.*t2.*t8.*t19.*t20.*6.4e1; 
+  // 'SteadyStateAcceleration:280' t274 = COM_Y.*q3.*q4.*rk.*t8.*t9.*t19.*t20.*6.4e1; 
+  // 'SteadyStateAcceleration:281' t275 = COM_Y.*Jk.*Mb.*q1.*q2.*rk.*t9.*t20.*6.4e1; 
+  // 'SteadyStateAcceleration:282' t276 = COM_Y.*Jk.*Mb.*q1.*q2.*rk.*t2.*t20.*6.4e1; 
+  // 'SteadyStateAcceleration:283' t277 = COM_X.*Jk.*Mb.*q2.*q4.*rk.*t10.*t20.*6.4e1; 
+  // 'SteadyStateAcceleration:284' t278 = COM_X.*Jk.*Mb.*q2.*q4.*rk.*t9.*t20.*6.4e1; 
+  // 'SteadyStateAcceleration:285' t279 = COM_Y.*Jk.*Mb.*q3.*q4.*rk.*t10.*t20.*6.4e1; 
+  // 'SteadyStateAcceleration:286' t280 = COM_Y.*Jk.*Mb.*q3.*q4.*rk.*t3.*t20.*6.4e1; 
+  // 'SteadyStateAcceleration:287' t281 = t2.*t4.*t8.*t9.*t19.*t20.*t21.*1.28e2; 
+  // 'SteadyStateAcceleration:288' t282 = t2.*t3.*t8.*t12.*t19.*t20.*t21.*6.4e1; 
+  // 'SteadyStateAcceleration:289' t283 = t3.*t7.*t8.*t9.*t19.*t20.*t21.*1.28e2; 
+  // 'SteadyStateAcceleration:290' t284 = t2.*t8.*t9.*t11.*t19.*t20.*t21.*1.28e2; 
+  // 'SteadyStateAcceleration:291' t285 = t2.*t8.*t10.*t12.*t19.*t20.*t21.*1.28e2; 
+  // 'SteadyStateAcceleration:292' t286 = t7.*t8.*t9.*t10.*t19.*t20.*t21.*6.4e1; 
+  // 'SteadyStateAcceleration:293' t287 = t2.*t4.*t8.*t9.*t19.*t20.*t23.*6.4e1;
+  // 'SteadyStateAcceleration:294' t288 = t2.*t3.*t8.*t12.*t19.*t20.*t23.*1.28e2; 
+  // 'SteadyStateAcceleration:295' t289 = t3.*t7.*t8.*t9.*t19.*t20.*t23.*1.28e2; 
+  // 'SteadyStateAcceleration:296' t290 = t2.*t3.*t8.*t11.*t19.*t20.*t21.*6.4e1; 
+  // 'SteadyStateAcceleration:297' t291 = t2.*t4.*t8.*t10.*t19.*t20.*t21.*1.28e2; 
+  // 'SteadyStateAcceleration:298' t292 = t3.*t7.*t8.*t10.*t19.*t20.*t21.*1.28e2; 
+  // 'SteadyStateAcceleration:299' t293 = t2.*t8.*t9.*t11.*t19.*t20.*t23.*6.4e1; 
+  // 'SteadyStateAcceleration:300' t294 = t2.*t8.*t10.*t12.*t19.*t20.*t23.*1.28e2; 
+  // 'SteadyStateAcceleration:301' t295 = t7.*t8.*t9.*t10.*t19.*t20.*t23.*1.28e2; 
+  // 'SteadyStateAcceleration:302' t296 = t3.*t8.*t9.*t11.*t19.*t20.*t21.*1.28e2; 
+  // 'SteadyStateAcceleration:303' t297 = t4.*t8.*t9.*t10.*t19.*t20.*t21.*6.4e1; 
+  // 'SteadyStateAcceleration:304' t298 = t3.*t8.*t10.*t12.*t19.*t20.*t21.*1.28e2; 
+  // 'SteadyStateAcceleration:305' t299 = t2.*t3.*t8.*t11.*t19.*t20.*t23.*1.28e2; 
+  // 'SteadyStateAcceleration:306' t300 = t2.*t4.*t8.*t10.*t19.*t20.*t23.*1.28e2; 
+  // 'SteadyStateAcceleration:307' t301 = t3.*t7.*t8.*t10.*t19.*t20.*t23.*6.4e1; 
+  // 'SteadyStateAcceleration:308' t302 = t3.*t8.*t9.*t11.*t19.*t20.*t23.*1.28e2; 
+  // 'SteadyStateAcceleration:309' t303 = t4.*t8.*t9.*t10.*t19.*t20.*t23.*1.28e2; 
+  // 'SteadyStateAcceleration:310' t304 = t3.*t8.*t10.*t12.*t19.*t20.*t23.*6.4e1; 
+  // 'SteadyStateAcceleration:311' t305 = COM_X.*COM_Z.*q1.*q3.*t2.*t4.*t8.*t19.*t20.*6.4e1; 
+  // 'SteadyStateAcceleration:312' t306 = COM_Y.*COM_Z.*q1.*q2.*t7.*t8.*t9.*t19.*t20.*6.4e1; 
+  // 'SteadyStateAcceleration:313' t307 = COM_X.*COM_Z.*q2.*q4.*t8.*t9.*t11.*t19.*t20.*6.4e1; 
+  // 'SteadyStateAcceleration:314' t308 = COM_Y.*COM_Z.*q3.*q4.*t3.*t8.*t11.*t19.*t20.*6.4e1; 
+  // 'SteadyStateAcceleration:315' t309 = COM_X.*Jk.*Mb.*q2.*q4.*rk.*t2.*t20.*6.4e1; 
+  // 'SteadyStateAcceleration:316' t310 = COM_Y.*Jk.*Mb.*q1.*q2.*rk.*t3.*t20.*6.4e1; 
+  // 'SteadyStateAcceleration:317' t311 = COM_Y.*Jk.*Mb.*q1.*q2.*rk.*t10.*t20.*6.4e1; 
+  // 'SteadyStateAcceleration:318' t312 = COM_X.*Jk.*Mb.*q2.*q4.*rk.*t3.*t20.*6.4e1; 
+  // 'SteadyStateAcceleration:319' t313 = COM_Y.*Jk.*Mb.*q3.*q4.*rk.*t2.*t20.*6.4e1; 
+  // 'SteadyStateAcceleration:320' t314 = COM_Y.*Jk.*Mb.*q3.*q4.*rk.*t9.*t20.*6.4e1; 
+  // 'SteadyStateAcceleration:321' t315 = COM_X.*COM_Y.*q1.*q4.*t8.*t9.*t11.*t19.*t20.*1.28e2; 
+  // 'SteadyStateAcceleration:322' t316 = COM_X.*COM_Y.*q1.*q4.*t8.*t10.*t12.*t19.*t20.*2.56e2; 
+  // 'SteadyStateAcceleration:323' t317 = COM_X.*COM_Y.*q1.*q4.*t2.*t8.*t12.*t19.*t20.*2.56e2; 
+  // 'SteadyStateAcceleration:324' t318 = COM_X.*COM_Y.*q1.*q4.*t7.*t8.*t9.*t19.*t20.*1.28e2; 
+  // 'SteadyStateAcceleration:325' t319 = COM_X.*COM_Z.*q1.*q3.*t4.*t8.*t9.*t19.*t20.*1.92e2; 
+  // 'SteadyStateAcceleration:326' t320 = COM_X.*COM_Z.*q1.*q3.*t3.*t8.*t12.*t19.*t20.*1.92e2; 
+  // 'SteadyStateAcceleration:327' t321 = COM_X.*COM_Z.*q1.*q3.*t2.*t8.*t12.*t19.*t20.*6.4e1; 
+  // 'SteadyStateAcceleration:328' t322 = COM_X.*COM_Z.*q2.*q4.*t2.*t8.*t11.*t19.*t20.*1.92e2; 
+  // 'SteadyStateAcceleration:329' t323 = COM_X.*COM_Z.*q2.*q4.*t7.*t8.*t10.*t19.*t20.*1.92e2; 
+  // 'SteadyStateAcceleration:330' t324 = COM_X.*COM_Z.*q2.*q4.*t7.*t8.*t9.*t19.*t20.*6.4e1; 
+  // 'SteadyStateAcceleration:331' t325 = COM_Y.*COM_Z.*q1.*q2.*t3.*t7.*t8.*t19.*t20.*6.4e1; 
+  // 'SteadyStateAcceleration:332' t326 = COM_X.*COM_Y.*q2.*q3.*t3.*t8.*t11.*t19.*t20.*2.56e2; 
+  // 'SteadyStateAcceleration:333' t327 = COM_X.*COM_Y.*q2.*q3.*t4.*t8.*t10.*t19.*t20.*1.28e2; 
+  // 'SteadyStateAcceleration:334' t328 = COM_X.*COM_Y.*q2.*q3.*t8.*t9.*t11.*t19.*t20.*2.56e2; 
+  // 'SteadyStateAcceleration:335' t329 = COM_X.*COM_Y.*q2.*q3.*t8.*t10.*t12.*t19.*t20.*1.28e2; 
+  // 'SteadyStateAcceleration:336' t330 = COM_X.*COM_Z.*q1.*q3.*t4.*t8.*t10.*t19.*t20.*6.4e1; 
+  // 'SteadyStateAcceleration:337' t331 = COM_Y.*COM_Z.*q1.*q2.*t8.*t9.*t11.*t19.*t20.*6.4e1; 
+  // 'SteadyStateAcceleration:338' t332 = COM_Y.*COM_Z.*q1.*q2.*t2.*t8.*t11.*t19.*t20.*1.92e2; 
+  // 'SteadyStateAcceleration:339' t333 = COM_Y.*COM_Z.*q1.*q2.*t7.*t8.*t10.*t19.*t20.*1.92e2; 
+  // 'SteadyStateAcceleration:340' t334 = COM_X.*COM_Z.*q2.*q4.*t3.*t8.*t11.*t19.*t20.*6.4e1; 
+  // 'SteadyStateAcceleration:341' t335 = COM_Y.*COM_Z.*q3.*q4.*t2.*t8.*t11.*t19.*t20.*1.92e2; 
+  // 'SteadyStateAcceleration:342' t336 = COM_Y.*COM_Z.*q3.*q4.*t7.*t8.*t10.*t19.*t20.*1.92e2; 
+  // 'SteadyStateAcceleration:343' t337 = COM_Y.*COM_Z.*q3.*q4.*t3.*t7.*t8.*t19.*t20.*6.4e1; 
+  // 'SteadyStateAcceleration:344' t338 = COM_Y.*COM_Z.*q3.*q4.*t8.*t9.*t11.*t19.*t20.*6.4e1; 
+  // 'SteadyStateAcceleration:345' t339 = COM_X.*Jw.*Mb.*q2.*q4.*rk.*t2.*t8.*t9.*t10.*t18.*4.8e2; 
+  // 'SteadyStateAcceleration:346' t340 = COM_Y.*Jw.*Mb.*q1.*q2.*rk.*t2.*t3.*t8.*t9.*t18.*4.8e2; 
+  // 'SteadyStateAcceleration:347' t341 = COM_X.*Jw.*Mb.*q1.*q3.*rk.*t2.*t4.*t8.*t9.*t18.*8.64e2; 
+  // 'SteadyStateAcceleration:348' t342 = COM_X.*Jw.*Mb.*q1.*q3.*rk.*t2.*t3.*t8.*t12.*t18.*8.64e2; 
+  // 'SteadyStateAcceleration:349' t343 = COM_X.*Jw.*Mb.*q1.*q3.*rk.*t3.*t7.*t8.*t9.*t18.*9.6e2; 
+  // 'SteadyStateAcceleration:350' t344 = COM_Y.*Jw.*Mb.*q1.*q2.*rk.*t2.*t8.*t9.*t10.*t18.*4.8e2; 
+  // 'SteadyStateAcceleration:351' t345 = COM_X.*Jw.*Mb.*q2.*q4.*rk.*t3.*t8.*t9.*t10.*t18.*4.8e2; 
+  // 'SteadyStateAcceleration:352' t346 = COM_Y.*Jw.*Mb.*q3.*q4.*rk.*t2.*t3.*t8.*t10.*t18.*4.8e2; 
+  // 'SteadyStateAcceleration:353' t347 = COM_Z.*Jw.*Mb.*rk.*t2.*t8.*t9.*t11.*t18.*2.88e2; 
+  // 'SteadyStateAcceleration:354' t348 = COM_Z.*Jw.*Mb.*rk.*t7.*t8.*t9.*t10.*t18.*2.88e2; 
+  // 'SteadyStateAcceleration:355' t349 = COM_X.*Jw.*Mb.*q1.*q3.*rk.*t2.*t3.*t8.*t11.*t18.*8.64e2; 
+  // 'SteadyStateAcceleration:356' t350 = COM_X.*Jw.*Mb.*q1.*q3.*rk.*t2.*t4.*t8.*t10.*t18.*9.6e2; 
+  // 'SteadyStateAcceleration:357' t351 = COM_X.*Jw.*Mb.*q1.*q3.*rk.*t3.*t7.*t8.*t10.*t18.*8.64e2; 
+  // 'SteadyStateAcceleration:358' t352 = COM_Y.*Jw.*Mb.*q3.*q4.*rk.*t3.*t8.*t9.*t10.*t18.*4.8e2; 
+  // 'SteadyStateAcceleration:359' t353 = COM_Z.*Jw.*Mb.*rk.*t2.*t3.*t4.*t8.*t9.*t18.*4.8e2; 
+  // 'SteadyStateAcceleration:360' t354 = COM_Z.*Jw.*Mb.*rk.*t2.*t3.*t8.*t11.*t18.*2.88e2; 
+  // 'SteadyStateAcceleration:361' t355 = COM_Z.*Jw.*Mb.*rk.*t2.*t4.*t8.*t12.*t18.*7.2e2; 
+  // 'SteadyStateAcceleration:362' t356 = COM_Z.*Jw.*Mb.*rk.*t2.*t3.*t8.*t9.*t12.*t18.*4.8e2; 
+  // 'SteadyStateAcceleration:363' t357 = COM_Z.*Jw.*Mb.*rk.*t4.*t7.*t8.*t9.*t18.*2.88e2; 
+  // 'SteadyStateAcceleration:364' t358 = COM_Z.*Jw.*Mb.*rk.*t3.*t7.*t8.*t10.*t18.*2.88e2; 
+  // 'SteadyStateAcceleration:365' t359 = COM_Z.*Jw.*Mb.*rk.*t3.*t7.*t8.*t12.*t18.*2.88e2; 
+  // 'SteadyStateAcceleration:366' t360 = COM_Z.*Jw.*Mb.*rk.*t2.*t8.*t9.*t10.*t12.*t18.*1.92e2; 
+  // 'SteadyStateAcceleration:367' t361 = COM_Z.*Jw.*Mb.*rk.*t2.*t3.*t4.*t8.*t10.*t18.*1.92e2; 
+  // 'SteadyStateAcceleration:368' t362 = COM_Z.*Jw.*Mb.*rk.*t4.*t8.*t9.*t11.*t18.*2.88e2; 
+  // 'SteadyStateAcceleration:369' t363 = COM_Z.*Jw.*Mb.*rk.*t3.*t4.*t8.*t9.*t10.*t18.*4.8e2; 
+  // 'SteadyStateAcceleration:370' t364 = COM_Z.*Jw.*Mb.*rk.*t3.*t8.*t11.*t12.*t18.*2.88e2; 
+  // 'SteadyStateAcceleration:371' t365 = COM_Z.*Jw.*Mb.*rk.*t4.*t8.*t10.*t12.*t18.*7.2e2; 
+  // 'SteadyStateAcceleration:372' t366 = COM_Z.*Jw.*Mb.*rk.*t3.*t8.*t9.*t10.*t12.*t18.*4.8e2; 
+  // 'SteadyStateAcceleration:373' t367 = q1.*q2.*q3.*q4.*t8.*t11.*t19.*t20.*t23.*1.28e2; 
+  // 'SteadyStateAcceleration:374' t368 = q1.*q2.*q3.*q4.*t4.*t8.*t19.*t20.*t23.*1.28e2; 
+  // 'SteadyStateAcceleration:375' t369 = q1.*q2.*q3.*q4.*t8.*t12.*t19.*t20.*t23.*1.28e2; 
+  // 'SteadyStateAcceleration:376' t370 = q1.*q2.*q3.*q4.*t7.*t8.*t19.*t20.*t23.*1.28e2; 
+  // 'SteadyStateAcceleration:377' t371 = COM_X.*Jw.*Mb.*q2.*q4.*rk.*t2.*t7.*t8.*t18.*4.8e1; 
+  // 'SteadyStateAcceleration:378' t372 = COM_Y.*Jw.*Mb.*q1.*q2.*rk.*t3.*t4.*t8.*t18.*4.8e1; 
+  // 'SteadyStateAcceleration:379' t373 = COM_X.*Jw.*Mb.*q1.*q3.*rk.*t8.*t16.*t18.*4.8e1; 
+  // 'SteadyStateAcceleration:380' t374 = COM_Y.*Jw.*Mb.*q1.*q2.*rk.*t8.*t10.*t11.*t18.*4.8e1; 
+  // 'SteadyStateAcceleration:381' t375 = COM_X.*Jw.*Mb.*q2.*q4.*rk.*t3.*t4.*t8.*t18.*4.8e1; 
+  // 'SteadyStateAcceleration:382' t376 = COM_Y.*Jw.*Mb.*q3.*q4.*rk.*t2.*t7.*t8.*t18.*4.8e1; 
+  // 'SteadyStateAcceleration:383' t377 = COM_X.*Jw.*Mb.*q1.*q3.*rk.*t8.*t15.*t18.*4.8e1; 
+  // 'SteadyStateAcceleration:384' t378 = COM_Y.*Jw.*Mb.*q3.*q4.*rk.*t8.*t9.*t12.*t18.*4.8e1; 
+  // 'SteadyStateAcceleration:385' t379 = COM_X.*Mb.*Mk.*q2.*q4.*rk.*t2.*t8.*t20.*6.4e1; 
+  // 'SteadyStateAcceleration:386' t380 = COM_Y.*Mb.*Mk.*q1.*q2.*rk.*t3.*t8.*t20.*6.4e1; 
+  // 'SteadyStateAcceleration:387' t381 = COM_Y.*Mb.*Mk.*q1.*q2.*rk.*t8.*t10.*t20.*6.4e1; 
+  // 'SteadyStateAcceleration:388' t382 = COM_X.*Mb.*Mk.*q2.*q4.*rk.*t3.*t8.*t20.*6.4e1; 
+  // 'SteadyStateAcceleration:389' t383 = COM_Y.*Mb.*Mk.*q3.*q4.*rk.*t2.*t8.*t20.*6.4e1; 
+  // 'SteadyStateAcceleration:390' t384 = COM_Y.*Mb.*Mk.*q3.*q4.*rk.*t8.*t9.*t20.*6.4e1; 
+  // 'SteadyStateAcceleration:391' t385 = t2.*t3.*t8.*t9.*t10.*t19.*t20.*t21.*2.56e2; 
+  // 'SteadyStateAcceleration:392' t386 = t2.*t3.*t8.*t9.*t10.*t19.*t20.*t23.*2.56e2; 
+  // 'SteadyStateAcceleration:393' t387 = COM_X.*COM_Y.*q1.*q4.*t2.*t8.*t9.*t10.*t19.*t20.*2.56e2; 
+  // 'SteadyStateAcceleration:394' t388 = COM_X.*COM_Z.*q1.*q3.*t2.*t3.*t8.*t9.*t19.*t20.*1.28e2; 
+  // 'SteadyStateAcceleration:395' t389 = COM_X.*COM_Z.*q2.*q4.*t2.*t8.*t9.*t10.*t19.*t20.*1.28e2; 
+  // 'SteadyStateAcceleration:396' t390 = COM_X.*COM_Y.*q2.*q3.*t3.*t8.*t9.*t10.*t19.*t20.*2.56e2; 
+  // 'SteadyStateAcceleration:397' t391 = COM_Y.*COM_Z.*q1.*q2.*t2.*t8.*t9.*t10.*t19.*t20.*1.28e2; 
+  // 'SteadyStateAcceleration:398' t392 = COM_Y.*COM_Z.*q3.*q4.*t2.*t3.*t8.*t10.*t19.*t20.*1.28e2; 
+  // 'SteadyStateAcceleration:399' t393 = q1.*q2.*q3.*q4.*t3.*t8.*t10.*t19.*t20.*t23.*2.56e2; 
+  // 'SteadyStateAcceleration:400' t394 = q1.*q2.*q3.*q4.*t8.*t9.*t10.*t19.*t20.*t23.*2.56e2; 
+  // 'SteadyStateAcceleration:401' t395 = q1.*q2.*q3.*q4.*t3.*t8.*t9.*t19.*t20.*t23.*2.56e2; 
+  // 'SteadyStateAcceleration:402' t396 = q1.*q2.*q3.*q4.*t2.*t8.*t10.*t19.*t20.*t23.*2.56e2; 
+  // 'SteadyStateAcceleration:403' t397 = q1.*q2.*q3.*q4.*t2.*t3.*t8.*t19.*t20.*t23.*2.56e2; 
+  // 'SteadyStateAcceleration:404' t398 = q1.*q2.*q3.*q4.*t2.*t8.*t9.*t19.*t20.*t23.*2.56e2; 
+  // 'SteadyStateAcceleration:405' t399 = COM_X.*COM_Y.*q1.*q4.*t8.*t9.*t12.*t19.*t20.*1.28e2; 
+  // 'SteadyStateAcceleration:406' t400 = COM_X.*COM_Z.*q1.*q3.*t8.*t9.*t12.*t19.*t20.*6.4e1; 
+  // 'SteadyStateAcceleration:407' t401 = COM_X.*COM_Z.*q2.*q4.*t2.*t7.*t8.*t19.*t20.*6.4e1; 
+  // 'SteadyStateAcceleration:408' t402 = COM_X.*COM_Y.*q2.*q3.*t8.*t10.*t11.*t19.*t20.*1.28e2; 
+  // 'SteadyStateAcceleration:409' t403 = COM_Y.*COM_Z.*q1.*q2.*t8.*t10.*t11.*t19.*t20.*6.4e1; 
+  // 'SteadyStateAcceleration:410' t404 = COM_Y.*COM_Z.*q3.*q4.*t2.*t7.*t8.*t19.*t20.*6.4e1; 
+  // 'SteadyStateAcceleration:411' t405 = COM_X.*Jw.*Mb.*q2.*q4.*rk.*t2.*t8.*t11.*t18.*1.44e2; 
+  // 'SteadyStateAcceleration:412' t406 = COM_X.*Jw.*Mb.*q2.*q4.*rk.*t2.*t8.*t12.*t18.*2.4e2; 
+  // 'SteadyStateAcceleration:413' t407 = COM_X.*Jw.*Mb.*q2.*q4.*rk.*t7.*t8.*t10.*t18.*1.44e2; 
+  // 'SteadyStateAcceleration:414' t408 = COM_X.*Jw.*Mb.*q2.*q4.*rk.*t7.*t8.*t9.*t18.*2.4e2; 
+  // 'SteadyStateAcceleration:415' t409 = COM_Y.*Jw.*Mb.*q1.*q2.*rk.*t4.*t8.*t9.*t18.*1.44e2; 
+  // 'SteadyStateAcceleration:416' t410 = COM_Y.*Jw.*Mb.*q1.*q2.*rk.*t3.*t8.*t12.*t18.*1.44e2; 
+  // 'SteadyStateAcceleration:417' t411 = COM_Y.*Jw.*Mb.*q1.*q2.*rk.*t2.*t4.*t8.*t18.*2.4e2; 
+  // 'SteadyStateAcceleration:418' t412 = COM_Y.*Jw.*Mb.*q1.*q2.*rk.*t3.*t7.*t8.*t18.*2.4e2; 
+  // 'SteadyStateAcceleration:419' t413 = COM_X.*Jw.*Mb.*q1.*q3.*rk.*t3.*t4.*t8.*t9.*t18.*1.92e2; 
+  // 'SteadyStateAcceleration:420' t414 = COM_X.*Jw.*Mb.*q1.*q3.*rk.*t4.*t8.*t12.*t18.*2.88e2; 
+  // 'SteadyStateAcceleration:421' t415 = COM_X.*Jw.*Mb.*q1.*q3.*rk.*t3.*t8.*t9.*t12.*t18.*1.92e2; 
+  // 'SteadyStateAcceleration:422' t416 = COM_X.*Jw.*Mb.*q1.*q3.*rk.*t2.*t8.*t9.*t12.*t18.*2.88e2; 
+  // 'SteadyStateAcceleration:423' t417 = COM_X.*Jw.*Mb.*q1.*q3.*rk.*t7.*t8.*t12.*t18.*4.8e2; 
+  // 'SteadyStateAcceleration:424' t418 = COM_X.*Jw.*Mb.*q1.*q3.*rk.*t2.*t7.*t8.*t9.*t18.*2.88e2; 
+  // 'SteadyStateAcceleration:425' t419 = COM_Y.*Jw.*Mb.*q1.*q2.*rk.*t8.*t9.*t11.*t18.*2.4e2; 
+  // 'SteadyStateAcceleration:426' t420 = COM_Y.*Jw.*Mb.*q1.*q2.*rk.*t8.*t10.*t12.*t18.*2.4e2; 
+  // 'SteadyStateAcceleration:427' t421 = COM_Y.*Jw.*Mb.*q1.*q2.*rk.*t2.*t8.*t11.*t18.*1.44e2; 
+  // 'SteadyStateAcceleration:428' t422 = COM_Y.*Jw.*Mb.*q1.*q2.*rk.*t7.*t8.*t10.*t18.*1.44e2; 
+  // 'SteadyStateAcceleration:429' t423 = COM_X.*Jw.*Mb.*q2.*q4.*rk.*t3.*t8.*t11.*t18.*2.4e2; 
+  // 'SteadyStateAcceleration:430' t424 = COM_X.*Jw.*Mb.*q2.*q4.*rk.*t4.*t8.*t10.*t18.*2.4e2; 
+  // 'SteadyStateAcceleration:431' t425 = COM_X.*Jw.*Mb.*q2.*q4.*rk.*t4.*t8.*t9.*t18.*1.44e2; 
+  // 'SteadyStateAcceleration:432' t426 = COM_X.*Jw.*Mb.*q2.*q4.*rk.*t3.*t8.*t12.*t18.*1.44e2; 
+  // 'SteadyStateAcceleration:433' t427 = COM_Y.*Jw.*Mb.*q3.*q4.*rk.*t2.*t8.*t11.*t18.*1.44e2; 
+  // 'SteadyStateAcceleration:434' t428 = COM_Y.*Jw.*Mb.*q3.*q4.*rk.*t2.*t4.*t8.*t18.*2.4e2; 
+  // 'SteadyStateAcceleration:435' t429 = COM_Y.*Jw.*Mb.*q3.*q4.*rk.*t7.*t8.*t10.*t18.*1.44e2; 
+  // 'SteadyStateAcceleration:436' t430 = COM_Y.*Jw.*Mb.*q3.*q4.*rk.*t3.*t7.*t8.*t18.*2.4e2; 
+  // 'SteadyStateAcceleration:437' t431 = COM_X.*Jw.*Mb.*q1.*q3.*rk.*t3.*t8.*t10.*t11.*t18.*2.88e2; 
+  // 'SteadyStateAcceleration:438' t432 = COM_X.*Jw.*Mb.*q1.*q3.*rk.*t4.*t8.*t11.*t18.*4.8e2; 
+  // 'SteadyStateAcceleration:439' t433 = COM_X.*Jw.*Mb.*q1.*q3.*rk.*t3.*t4.*t8.*t10.*t18.*2.88e2; 
+  // 'SteadyStateAcceleration:440' t434 = COM_X.*Jw.*Mb.*q1.*q3.*rk.*t2.*t8.*t10.*t11.*t18.*1.92e2; 
+  // 'SteadyStateAcceleration:441' t435 = COM_X.*Jw.*Mb.*q1.*q3.*rk.*t7.*t8.*t11.*t18.*2.88e2; 
+  // 'SteadyStateAcceleration:442' t436 = COM_X.*Jw.*Mb.*q1.*q3.*rk.*t2.*t7.*t8.*t10.*t18.*1.92e2; 
+  // 'SteadyStateAcceleration:443' t437 = COM_Y.*Jw.*Mb.*q3.*q4.*rk.*t8.*t9.*t11.*t18.*2.4e2; 
+  // 'SteadyStateAcceleration:444' t438 = COM_Y.*Jw.*Mb.*q3.*q4.*rk.*t4.*t8.*t9.*t18.*1.44e2; 
+  // 'SteadyStateAcceleration:445' t439 = COM_Y.*Jw.*Mb.*q3.*q4.*rk.*t8.*t10.*t12.*t18.*2.4e2; 
+  // 'SteadyStateAcceleration:446' t440 = COM_Y.*Jw.*Mb.*q3.*q4.*rk.*t3.*t8.*t12.*t18.*1.44e2; 
+  // 'SteadyStateAcceleration:447' t441 = COM_X.*COM_Z.*q1.*q3.*t3.*t8.*t9.*t10.*t19.*t20.*1.28e2; 
+  // 'SteadyStateAcceleration:448' t442 = COM_X.*COM_Z.*q2.*q4.*t2.*t3.*t8.*t10.*t19.*t20.*1.28e2; 
+  // 'SteadyStateAcceleration:449' t443 = COM_Y.*COM_Z.*q3.*q4.*t2.*t8.*t9.*t10.*t19.*t20.*1.28e2; 
+  // 'SteadyStateAcceleration:450' t444 = COM_Y.*COM_Z.*q1.*q2.*t2.*t3.*t8.*t10.*t19.*t20.*1.28e2; 
+  // 'SteadyStateAcceleration:451' t445 = COM_X.*Jw.*Mb.*q1.*q3.*rk.*t2.*t3.*t8.*t9.*t10.*t18.*1.92e3; 
+  // 'SteadyStateAcceleration:452' t446 = COM_Z.*Jw.*Mb.*rk.*t2.*t4.*t8.*t9.*t10.*t18.*5.76e2; 
+  // 'SteadyStateAcceleration:453' t447 = COM_Z.*Jw.*Mb.*rk.*t2.*t3.*t8.*t10.*t12.*t18.*5.76e2; 
+  // 'SteadyStateAcceleration:454' t448 = COM_X.*Jw.*Mb.*q2.*q4.*rk.*t2.*t4.*t8.*t18.*2.4e2; 
+  // 'SteadyStateAcceleration:455' t449 = COM_X.*Jw.*Mb.*q2.*q4.*rk.*t3.*t7.*t8.*t18.*2.4e2; 
+  // 'SteadyStateAcceleration:456' t450 = COM_Y.*Jw.*Mb.*q1.*q2.*rk.*t3.*t8.*t11.*t18.*2.4e2; 
+  // 'SteadyStateAcceleration:457' t451 = COM_Y.*Jw.*Mb.*q1.*q2.*rk.*t4.*t8.*t10.*t18.*2.4e2; 
+  // 'SteadyStateAcceleration:458' t452 = COM_Y.*Jw.*Mb.*q3.*q4.*rk.*t2.*t8.*t12.*t18.*2.4e2; 
+  // 'SteadyStateAcceleration:459' t453 = COM_Y.*Jw.*Mb.*q3.*q4.*rk.*t7.*t8.*t9.*t18.*2.4e2; 
+  // 'SteadyStateAcceleration:460' t454 = COM_X.*Jw.*Mb.*q1.*q3.*rk.*t8.*t9.*t10.*t11.*t18.*2.88e2; 
+  // 'SteadyStateAcceleration:461' t455 = COM_X.*Jw.*Mb.*q1.*q3.*rk.*t8.*t11.*t12.*t18.*4.8e2; 
+  // 'SteadyStateAcceleration:462' t456 = COM_X.*Jw.*Mb.*q1.*q3.*rk.*t8.*t9.*t10.*t12.*t18.*2.88e2; 
+  // 'SteadyStateAcceleration:463' t457 = COM_X.*COM_Y.*q1.*q4.*t3.*t8.*t12.*t19.*t20.*1.28e2; 
+  // 'SteadyStateAcceleration:464' t458 = COM_X.*COM_Y.*q2.*q3.*t2.*t8.*t11.*t19.*t20.*1.28e2; 
+  // 'SteadyStateAcceleration:465' t459 = COM_X.*COM_Z.*q1.*q3.*t8.*t10.*t12.*t19.*t20.*6.4e1; 
+  // 'SteadyStateAcceleration:466' t460 = COM_X.*COM_Z.*q2.*q4.*t3.*t7.*t8.*t19.*t20.*6.4e1; 
+  // 'SteadyStateAcceleration:467' t461 = COM_Y.*COM_Z.*q1.*q2.*t3.*t8.*t11.*t19.*t20.*6.4e1; 
+  // 'SteadyStateAcceleration:468' t462 = COM_Y.*COM_Z.*q3.*q4.*t7.*t8.*t9.*t19.*t20.*6.4e1; 
+  // 'SteadyStateAcceleration:469' t463 = COM_X.*Jw.*Mb.*q2.*q4.*rk.*t2.*t3.*t8.*t10.*t18.*4.8e2; 
+  // 'SteadyStateAcceleration:470' t464 = COM_X.*Jw.*Mb.*q2.*q4.*rk.*t2.*t3.*t8.*t9.*t18.*4.8e2; 
+  // 'SteadyStateAcceleration:471' t465 = COM_Y.*Jw.*Mb.*q1.*q2.*rk.*t3.*t8.*t9.*t10.*t18.*4.8e2; 
+  // 'SteadyStateAcceleration:472' t466 = COM_Y.*Jw.*Mb.*q3.*q4.*rk.*t2.*t8.*t9.*t10.*t18.*4.8e2; 
+  // 'SteadyStateAcceleration:473' t467 = COM_Y.*Jw.*Mb.*q3.*q4.*rk.*t2.*t3.*t8.*t9.*t18.*4.8e2; 
+  // 'SteadyStateAcceleration:474' t468 = COM_Y.*Jw.*Mb.*q1.*q2.*rk.*t2.*t3.*t8.*t10.*t18.*4.8e2; 
+  // 'SteadyStateAcceleration:475' t469 = COM_X.*Jw.*Mb.*q1.*q3.*rk.*t3.*t8.*t9.*t11.*t18.*9.6e2; 
+  // 'SteadyStateAcceleration:476' t470 = COM_X.*Jw.*Mb.*q1.*q3.*rk.*t4.*t8.*t9.*t10.*t18.*8.64e2; 
+  // 'SteadyStateAcceleration:477' t471 = COM_X.*Jw.*Mb.*q1.*q3.*rk.*t3.*t8.*t10.*t12.*t18.*8.64e2; 
+  // 'SteadyStateAcceleration:478' t472 = COM_X.*Jw.*Mb.*q1.*q3.*rk.*t2.*t8.*t9.*t11.*t18.*8.64e2; 
+  // 'SteadyStateAcceleration:479' t473 = COM_X.*Jw.*Mb.*q1.*q3.*rk.*t2.*t8.*t10.*t12.*t18.*9.6e2; 
+  // 'SteadyStateAcceleration:480' t474 = COM_X.*Jw.*Mb.*q1.*q3.*rk.*t7.*t8.*t9.*t10.*t18.*8.64e2; 
+  // 'SteadyStateAcceleration:481' t475 = t26+t27+t28+t29+t30+t31+t32+t33+t34+t35+t37+t38+t39+t40+t41+t42+t43+t44+t45+t46+t47+t48+t49+t50+t51+t52+t53+t54+t55+t56+t57+t58+t59+t60+t61+t62+t63+t64+t65+t66+t67+t68+t69+t70+t71+t72+t73+t74+t75+t76+t77+t78+t79+t80+t81+t82+t83+t84+t85+t86+t87+t88+t89+t90+t91+t92+t93+t94+t95+t96+t97+t98+t99+t100+t101+t102+t103+t104+t105+t106+t107+t108+t109+t110+t111+t112+t113+t114+t115+t116+t117+t118+t119+t120+t121+t122+t123+t124+t125+t126+t127+t128+t129+t130+t131+t132+t133+t134+t135+t136+t137+t138+t139+t140+t141+t142+t143+t144+t145+t146+t147+t148+t149+t150+t151+t152+t153+t154+t155+t156+t157+t158+t159+t160+t161+t162+t163+t164+t165+t166+t167+t168+t169+t170+t171+t172+t173+t174+t175+t176+t177+t178+t179+t180+t181+t182+t183+t184+t185+t186+t187+t188+t189+t190+t191+t192+t193+t194+t195+t196+t197+t198+t199+t200+t201+t202+t203+t204+t205+t206+t207+t208+t209+t210+t211+t212+t213+t214+t215+t216+t217+t218+t219+t220+t221+t222+t223+t224+t225+t226+t227+t228+t229+t230+t231+t232+t233+t234+t235+t236+t237+t238+t239+t240+t241+t242+t243+t244+t245+t246+t247+t248+t249+t250+t251+t252+t253+t254+t255+t256+t257+t258+t259+t260+t261+t262+t263+t264+t265+t266+t267+t268+t269+t270+t271+t272+t273+t274+t275+t276+t277+t278+t279+t280+t281+t282+t283+t284+t285+t286+t287+t288+t289+t290+t291+t292+t293+t294+t295+t296+t297+t298+t299+t300+t301+t302+t303+t304+t305+t306+t307+t308+t309+t310+t311+t312+t313+t314+t315+t316+t317+t318+t319+t320+t321+t322+t323+t324+t325+t326+t327+t328+t329+t330+t331+t332+t333+t334+t335+t336+t337+t338+t339+t340+t341+t342+t343+t344+t345+t346+t347+t348+t349+t350+t351+t352+t353+t354+t355+t356+t357+t358+t359+t360+t361+t362+t363+t364+t365+t366+t367+t368+t369+t370+t371+t372+t373+t374+t375+t376+t377+t378+t379+t380+t381+t382+t383+t384+t385+t386+t387+t388+t389+t390+t391+t392+t393+t394+t395+t396+t397+t398+t399+t400+t401+t402+t403+t404+t405+t406+t407+t408+t409+t410+t411+t412+t413+t414+t415+t416+t417+t418+t419+t420+t421+t422+t423+t424+t425+t426+t427+t428+t429+t430+t431+t432+t433+t434+t435+t436+t437+t438+t439+t440+t441+t442+t443+t444+t445+t446+t447+t448+t449+t450+t451+t452+t453+t454+t455+t456+t457+t458+t459+t460+t461+t462+t463+t464+t465+t466+t467+t468+t469+t470+t471+t472+t473+t474-t2.*t5.*t13.*t24.*1.62e2-t2.*t5.*t14.*t24.*1.8e1-t3.*t5.*t13.*t24.*1.8e1-t2.*t5.*t15.*t24.*9.0e1-t3.*t5.*t14.*t24.*1.62e2-t2.*t5.*t16.*t24.*1.62e2-t3.*t5.*t15.*t24.*1.62e2-t3.*t5.*t16.*t24.*9.0e1-t5.*t9.*t13.*t24.*9.0e1-t5.*t9.*t14.*t24.*1.62e2-t5.*t10.*t13.*t24.*1.62e2-t5.*t9.*t15.*t24.*1.62e2-t5.*t10.*t14.*t24.*9.0e1-t5.*t9.*t16.*t24.*1.8e1-t5.*t10.*t15.*t24.*1.8e1-t5.*t10.*t16.*t24.*1.62e2-t2.*t4.*t5.*t7.*t24.*3.96e2-t3.*t4.*t5.*t7.*t24.*3.96e2-t2.*t4.*t5.*t11.*t24.*1.188e3-t2.*t4.*t5.*t12.*t24.*9.72e2-t3.*t4.*t5.*t11.*t24.*3.96e2-t3.*t4.*t5.*t12.*t24.*1.8e2-t2.*t5.*t7.*t11.*t24.*1.8e2-t4.*t5.*t7.*t9.*t24.*1.188e3-t2.*t5.*t7.*t12.*t24.*3.96e2-t3.*t5.*t7.*t11.*t24.*9.72e2-t4.*t5.*t7.*t10.*t24.*1.188e3-t3.*t5.*t7.*t12.*t24.*1.188e3-t4.*t5.*t9.*t11.*t24.*1.188e3-t2.*t5.*t11.*t12.*t24.*1.188e3-t4.*t5.*t9.*t12.*t24.*1.8e2-t4.*t5.*t10.*t11.*t24.*3.96e2-t3.*t5.*t11.*t12.*t24.*1.188e3-t4.*t5.*t10.*t12.*t24.*9.72e2-t5.*t7.*t9.*t11.*t24.*9.72e2-t5.*t7.*t9.*t12.*t24.*3.96e2-t5.*t7.*t10.*t11.*t24.*1.8e2-t5.*t7.*t10.*t12.*t24.*1.188e3-t5.*t9.*t11.*t12.*t24.*3.96e2-t5.*t10.*t11.*t12.*t24.*3.96e2-t2.*t3.*t4.*t5.*t9.*t24.*6.48e2-t2.*t3.*t4.*t5.*t10.*t24.*7.92e2-t2.*t3.*t5.*t7.*t9.*t24.*7.92e2-t2.*t3.*t5.*t7.*t10.*t24.*6.48e2-t2.*t3.*t5.*t9.*t11.*t24.*2.376e3-t2.*t4.*t5.*t9.*t10.*t24.*2.376e3-t2.*t3.*t5.*t9.*t12.*t24.*6.48e2-t2.*t3.*t5.*t10.*t11.*t24.*6.48e2-t3.*t4.*t5.*t9.*t10.*t24.*6.48e2-t2.*t3.*t5.*t10.*t12.*t24.*2.376e3-t2.*t5.*t7.*t9.*t10.*t24.*6.48e2-t3.*t5.*t7.*t9.*t10.*t24.*2.376e3-t2.*t5.*t9.*t10.*t11.*t24.*6.48e2-t2.*t5.*t9.*t10.*t12.*t24.*7.92e2-t3.*t5.*t9.*t10.*t11.*t24.*7.92e2-t3.*t5.*t9.*t10.*t12.*t24.*6.48e2-t4.*t7.*t8.*t19.*t20.*t22.*3.2e1-t4.*t8.*t11.*t19.*t20.*t22.*3.2e1-t7.*t8.*t12.*t19.*t20.*t22.*3.2e1-t8.*t11.*t12.*t19.*t20.*t22.*3.2e1-COM_Z.*Jk.*Mb.*rk.*t4.*t20.*3.2e1-COM_Z.*Jk.*Mb.*rk.*t12.*t20.*3.2e1-Jk.*Jw.*t2.*t4.*t8.*t18.*1.2e2-Jk.*Jw.*t3.*t4.*t8.*t18.*2.4e1-Jk.*Jw.*t2.*t7.*t8.*t18.*2.4e1-Jk.*Jw.*t3.*t7.*t8.*t18.*1.2e2-Jk.*Jw.*t2.*t8.*t11.*t18.*7.2e1-Jk.*Jw.*t4.*t8.*t9.*t18.*7.2e1-Jk.*Jw.*t2.*t8.*t12.*t18.*1.2e2-Jk.*Jw.*t3.*t8.*t11.*t18.*1.2e2-Jk.*Jw.*t4.*t8.*t10.*t18.*1.2e2-Jk.*Jw.*t3.*t8.*t12.*t18.*7.2e1-Jk.*Jw.*t7.*t8.*t9.*t18.*1.2e2-Jk.*Jw.*t7.*t8.*t10.*t18.*7.2e1-Jk.*Jw.*t8.*t9.*t11.*t18.*1.2e2-Jk.*Jw.*t8.*t9.*t12.*t18.*2.4e1-Jk.*Jw.*t8.*t10.*t11.*t18.*2.4e1-Jk.*Jw.*t8.*t10.*t12.*t18.*1.2e2-Jw.*Mb.*t2.*t4.*t18.*t24.*1.2e2-Jw.*Mb.*t3.*t4.*t18.*t24.*2.4e1-Jw.*Mb.*t2.*t7.*t18.*t24.*2.4e1-Jw.*Mb.*t3.*t7.*t18.*t24.*1.2e2-Jw.*Mb.*t2.*t11.*t18.*t24.*7.2e1-Jw.*Mb.*t4.*t9.*t18.*t24.*7.2e1-Jw.*Mb.*t2.*t12.*t18.*t24.*1.2e2-Jw.*Mb.*t3.*t11.*t18.*t24.*1.2e2-Jw.*Mb.*t4.*t10.*t18.*t24.*1.2e2-Jw.*Mb.*t3.*t12.*t18.*t24.*7.2e1-Jw.*Mb.*t7.*t9.*t18.*t24.*1.2e2-Jw.*Mb.*t7.*t10.*t18.*t24.*7.2e1-Jw.*Mb.*t9.*t11.*t18.*t24.*1.2e2-Jw.*Mb.*t9.*t12.*t18.*t24.*2.4e1-Jw.*Mb.*t10.*t11.*t18.*t24.*2.4e1-Jw.*Mb.*t10.*t12.*t18.*t24.*1.2e2-Jw.*Mk.*t2.*t4.*t18.*t24.*1.2e2-Jw.*Mk.*t3.*t4.*t18.*t24.*2.4e1-Jw.*Mk.*t2.*t7.*t18.*t24.*2.4e1-Jw.*Mk.*t3.*t7.*t18.*t24.*1.2e2-Jw.*Mk.*t2.*t11.*t18.*t24.*7.2e1-Jw.*Mk.*t4.*t9.*t18.*t24.*7.2e1-Jw.*Mk.*t2.*t12.*t18.*t24.*1.2e2-Jw.*Mk.*t3.*t11.*t18.*t24.*1.2e2-Jw.*Mk.*t4.*t10.*t18.*t24.*1.2e2-Jw.*Mk.*t3.*t12.*t18.*t24.*7.2e1-Jw.*Mk.*t7.*t9.*t18.*t24.*1.2e2-Jw.*Mk.*t7.*t10.*t18.*t24.*7.2e1-Jw.*Mk.*t9.*t11.*t18.*t24.*1.2e2-Jw.*Mk.*t9.*t12.*t18.*t24.*2.4e1-Jw.*Mk.*t10.*t11.*t18.*t24.*2.4e1-Jw.*Mk.*t10.*t12.*t18.*t24.*1.2e2-COM_Z.*rk.*t4.*t8.*t19.*t20.*3.2e1-COM_Z.*rk.*t8.*t12.*t19.*t20.*3.2e1-COM_Z.*Jk.*Mb.*rk.*t3.*t9.*t20.*6.4e1-COM_Z.*Jw.*Mb.*rk.*t8.*t13.*t18.*2.4e1-COM_Z.*Jw.*Mb.*rk.*t8.*t16.*t18.*2.4e1-COM_Z.*Mb.*Mk.*rk.*t4.*t8.*t20.*3.2e1-COM_Z.*Mb.*Mk.*rk.*t8.*t12.*t20.*3.2e1-Jk.*Jw.*t2.*t3.*t8.*t9.*t18.*2.4e2-Jk.*Jw.*t2.*t3.*t8.*t10.*t18.*2.4e2-Jk.*Jw.*t2.*t8.*t9.*t10.*t18.*2.4e2-Jk.*Jw.*t3.*t8.*t9.*t10.*t18.*2.4e2-Jw.*Mb.*t2.*t3.*t9.*t18.*t24.*2.4e2-Jw.*Mb.*t2.*t3.*t10.*t18.*t24.*2.4e2-Jw.*Mb.*t2.*t9.*t10.*t18.*t24.*2.4e2-Jw.*Mb.*t3.*t9.*t10.*t18.*t24.*2.4e2-Jw.*Mk.*t2.*t3.*t9.*t18.*t24.*2.4e2-Jw.*Mk.*t2.*t3.*t10.*t18.*t24.*2.4e2-Jw.*Mk.*t2.*t9.*t10.*t18.*t24.*2.4e2-Jw.*Mk.*t3.*t9.*t10.*t18.*t24.*2.4e2-COM_Z.*rk.*t3.*t8.*t9.*t19.*t20.*6.4e1-t2.*t4.*t8.*t10.*t19.*t20.*t22.*6.4e1-t3.*t7.*t8.*t9.*t19.*t20.*t22.*6.4e1-t3.*t8.*t9.*t11.*t19.*t20.*t22.*6.4e1-t2.*t8.*t10.*t12.*t19.*t20.*t22.*6.4e1-COM_X.*Jk.*Mb.*q1.*q3.*rk.*t2.*t20.*6.4e1-COM_X.*Jk.*Mb.*q1.*q3.*rk.*t3.*t20.*6.4e1-COM_X.*Jk.*Mb.*q1.*q3.*rk.*t9.*t20.*6.4e1-COM_X.*Jk.*Mb.*q1.*q3.*rk.*t10.*t20.*6.4e1-COM_Z.*Jw.*Mb.*rk.*t2.*t8.*t14.*t18.*2.4e1-COM_Z.*Jw.*Mb.*rk.*t4.*t8.*t12.*t18.*1.44e2-COM_Z.*Jw.*Mb.*rk.*t2.*t8.*t15.*t18.*1.2e2-COM_Z.*Jw.*Mb.*rk.*t3.*t8.*t14.*t18.*1.2e2-COM_Z.*Jw.*Mb.*rk.*t3.*t8.*t15.*t18.*1.2e2-COM_Z.*Jw.*Mb.*rk.*t8.*t9.*t14.*t18.*1.2e2-COM_Z.*Jw.*Mb.*rk.*t8.*t9.*t15.*t18.*1.2e2-COM_Z.*Jw.*Mb.*rk.*t8.*t10.*t14.*t18.*1.2e2-COM_Z.*Jw.*Mb.*rk.*t8.*t10.*t15.*t18.*2.4e1-COM_Z.*Mb.*Mk.*rk.*t3.*t8.*t9.*t20.*6.4e1-COM_X.*q1.*q3.*rk.*t2.*t8.*t19.*t20.*6.4e1-COM_X.*q1.*q3.*rk.*t3.*t8.*t19.*t20.*6.4e1-COM_X.*q1.*q3.*rk.*t8.*t9.*t19.*t20.*6.4e1-COM_X.*q1.*q3.*rk.*t8.*t10.*t19.*t20.*6.4e1-t2.*t3.*t8.*t9.*t10.*t19.*t20.*t22.*1.28e2-COM_X.*COM_Y.*q1.*q4.*t2.*t4.*t8.*t19.*t20.*2.56e2-COM_X.*COM_Y.*q2.*q3.*t2.*t4.*t8.*t19.*t20.*1.28e2-COM_Y.*COM_Z.*q1.*q2.*t2.*t4.*t8.*t19.*t20.*6.4e1-COM_X.*COM_Y.*q1.*q4.*t3.*t4.*t8.*t19.*t20.*1.28e2-COM_Y.*COM_Z.*q1.*q2.*t3.*t4.*t8.*t19.*t20.*6.4e1-COM_X.*COM_Z.*q2.*q4.*t2.*t4.*t8.*t19.*t20.*6.4e1-COM_X.*COM_Y.*q2.*q3.*t2.*t7.*t8.*t19.*t20.*1.28e2-COM_X.*COM_Z.*q1.*q3.*t2.*t7.*t8.*t19.*t20.*6.4e1-COM_X.*COM_Z.*q2.*q4.*t3.*t4.*t8.*t19.*t20.*6.4e1-COM_X.*COM_Y.*q1.*q4.*t3.*t7.*t8.*t19.*t20.*1.28e2-COM_X.*COM_Y.*q2.*q3.*t3.*t7.*t8.*t19.*t20.*2.56e2-COM_X.*COM_Z.*q1.*q3.*t3.*t7.*t8.*t19.*t20.*6.4e1-COM_Y.*COM_Z.*q3.*q4.*t2.*t4.*t8.*t19.*t20.*6.4e1-COM_Y.*COM_Z.*q3.*q4.*t3.*t4.*t8.*t19.*t20.*6.4e1-COM_X.*COM_Y.*q1.*q4.*t4.*t8.*t9.*t19.*t20.*1.28e2-COM_X.*COM_Z.*q1.*q3.*t2.*t8.*t11.*t19.*t20.*1.92e2-COM_Y.*COM_Z.*q1.*q2.*t4.*t8.*t9.*t19.*t20.*1.92e2-COM_X.*COM_Y.*q1.*q4.*t3.*t8.*t11.*t19.*t20.*1.28e2-COM_X.*COM_Y.*q1.*q4.*t4.*t8.*t10.*t19.*t20.*2.56e2-COM_X.*COM_Y.*q2.*q3.*t2.*t8.*t12.*t19.*t20.*1.28e2-COM_X.*COM_Z.*q1.*q3.*t3.*t8.*t11.*t19.*t20.*6.4e1-COM_Y.*COM_Z.*q1.*q2.*t2.*t8.*t12.*t19.*t20.*6.4e1-COM_Y.*COM_Z.*q1.*q2.*t4.*t8.*t10.*t19.*t20.*6.4e1-COM_X.*COM_Z.*q2.*q4.*t4.*t8.*t9.*t19.*t20.*1.92e2-COM_Y.*COM_Z.*q1.*q2.*t3.*t8.*t12.*t19.*t20.*1.92e2-COM_X.*COM_Y.*q2.*q3.*t7.*t8.*t9.*t19.*t20.*2.56e2-COM_X.*COM_Z.*q1.*q3.*t7.*t8.*t9.*t19.*t20.*6.4e1-COM_X.*COM_Z.*q2.*q4.*t2.*t8.*t12.*t19.*t20.*6.4e1-COM_X.*COM_Z.*q2.*q4.*t4.*t8.*t10.*t19.*t20.*6.4e1-COM_X.*COM_Y.*q2.*q3.*t7.*t8.*t10.*t19.*t20.*1.28e2-COM_X.*COM_Z.*q1.*q3.*t7.*t8.*t10.*t19.*t20.*1.92e2-COM_X.*COM_Z.*q2.*q4.*t3.*t8.*t12.*t19.*t20.*1.92e2-COM_Y.*COM_Z.*q3.*q4.*t4.*t8.*t9.*t19.*t20.*1.92e2-COM_Y.*COM_Z.*q3.*q4.*t2.*t8.*t12.*t19.*t20.*6.4e1-COM_Y.*COM_Z.*q3.*q4.*t4.*t8.*t10.*t19.*t20.*6.4e1-COM_Y.*COM_Z.*q3.*q4.*t3.*t8.*t12.*t19.*t20.*1.92e2-COM_X.*COM_Z.*q1.*q3.*t8.*t9.*t11.*t19.*t20.*6.4e1-COM_X.*COM_Z.*q1.*q3.*t8.*t10.*t11.*t19.*t20.*6.4e1-COM_Y.*COM_Z.*q1.*q2.*t8.*t9.*t12.*t19.*t20.*6.4e1-COM_Y.*COM_Z.*q1.*q2.*t8.*t10.*t12.*t19.*t20.*6.4e1-COM_X.*COM_Z.*q2.*q4.*t8.*t9.*t12.*t19.*t20.*6.4e1-COM_X.*COM_Z.*q2.*q4.*t8.*t10.*t12.*t19.*t20.*6.4e1-COM_Y.*COM_Z.*q3.*q4.*t8.*t9.*t12.*t19.*t20.*6.4e1-COM_Y.*COM_Z.*q3.*q4.*t8.*t10.*t12.*t19.*t20.*6.4e1-q1.*q2.*q3.*q4.*t4.*t8.*t19.*t20.*t21.*1.28e2-q1.*q2.*q3.*q4.*t7.*t8.*t19.*t20.*t21.*1.28e2-q1.*q2.*q3.*q4.*t8.*t11.*t19.*t20.*t21.*1.28e2-q1.*q2.*q3.*q4.*t8.*t12.*t19.*t20.*t21.*1.28e2-COM_Y.*Jw.*Mb.*q1.*q2.*rk.*t8.*t13.*t18.*4.8e1-COM_Y.*Jw.*Mb.*q1.*q2.*rk.*t8.*t14.*t18.*4.8e1-COM_X.*Jw.*Mb.*q2.*q4.*rk.*t8.*t13.*t18.*4.8e1-COM_Y.*Jw.*Mb.*q1.*q2.*rk.*t8.*t15.*t18.*4.8e1-COM_X.*Jw.*Mb.*q2.*q4.*rk.*t8.*t14.*t18.*4.8e1-COM_Y.*Jw.*Mb.*q1.*q2.*rk.*t8.*t16.*t18.*4.8e1-COM_X.*Jw.*Mb.*q2.*q4.*rk.*t8.*t15.*t18.*4.8e1-COM_Y.*Jw.*Mb.*q3.*q4.*rk.*t8.*t13.*t18.*4.8e1-COM_X.*Jw.*Mb.*q2.*q4.*rk.*t8.*t16.*t18.*4.8e1-COM_Y.*Jw.*Mb.*q3.*q4.*rk.*t8.*t14.*t18.*4.8e1-COM_Y.*Jw.*Mb.*q3.*q4.*rk.*t8.*t15.*t18.*4.8e1-COM_Y.*Jw.*Mb.*q3.*q4.*rk.*t8.*t16.*t18.*4.8e1-COM_X.*Mb.*Mk.*q1.*q3.*rk.*t2.*t8.*t20.*6.4e1-COM_X.*Mb.*Mk.*q1.*q3.*rk.*t3.*t8.*t20.*6.4e1-COM_X.*Mb.*Mk.*q1.*q3.*rk.*t8.*t9.*t20.*6.4e1-COM_X.*Mb.*Mk.*q1.*q3.*rk.*t8.*t10.*t20.*6.4e1-COM_Z.*Jw.*Mb.*rk.*t2.*t3.*t4.*t8.*t18.*9.6e1-COM_Z.*Jw.*Mb.*rk.*t2.*t4.*t7.*t8.*t18.*9.6e1-COM_Z.*Jw.*Mb.*rk.*t2.*t4.*t8.*t9.*t18.*2.88e2-COM_Z.*Jw.*Mb.*rk.*t3.*t4.*t8.*t9.*t18.*9.6e1-COM_Z.*Jw.*Mb.*rk.*t2.*t3.*t8.*t12.*t18.*2.88e2-COM_Z.*Jw.*Mb.*rk.*t2.*t4.*t8.*t11.*t18.*2.88e2-COM_Z.*Jw.*Mb.*rk.*t3.*t4.*t8.*t10.*t18.*9.6e1-COM_Z.*Jw.*Mb.*rk.*t2.*t7.*t8.*t11.*t18.*2.4e2-COM_Z.*Jw.*Mb.*rk.*t2.*t7.*t8.*t12.*t18.*9.6e1-COM_Z.*Jw.*Mb.*rk.*t3.*t7.*t8.*t11.*t18.*7.2e2-COM_Z.*Jw.*Mb.*rk.*t4.*t7.*t8.*t10.*t18.*2.88e2-COM_Z.*Jw.*Mb.*rk.*t2.*t8.*t9.*t12.*t18.*9.6e1-COM_Z.*Jw.*Mb.*rk.*t4.*t8.*t9.*t10.*t18.*2.88e2-COM_Z.*Jw.*Mb.*rk.*t3.*t8.*t9.*t12.*t18.*9.6e1-COM_Z.*Jw.*Mb.*rk.*t2.*t8.*t11.*t12.*t18.*2.88e2-COM_Z.*Jw.*Mb.*rk.*t3.*t8.*t10.*t12.*t18.*2.88e2-COM_Z.*Jw.*Mb.*rk.*t4.*t8.*t10.*t11.*t18.*9.6e1-COM_Z.*Jw.*Mb.*rk.*t7.*t8.*t9.*t11.*t18.*7.2e2-COM_Z.*Jw.*Mb.*rk.*t7.*t8.*t10.*t11.*t18.*2.4e2-COM_Z.*Jw.*Mb.*rk.*t7.*t8.*t10.*t12.*t18.*2.88e2-COM_Z.*Jw.*Mb.*rk.*t8.*t9.*t10.*t12.*t18.*9.6e1-COM_Z.*Jw.*Mb.*rk.*t8.*t10.*t11.*t12.*t18.*9.6e1-COM_X.*Jw.*Mb.*q1.*q3.*rk.*t2.*t4.*t8.*t18.*2.4e2-COM_X.*Jw.*Mb.*q1.*q3.*rk.*t3.*t4.*t8.*t18.*4.8e1-COM_X.*Jw.*Mb.*q1.*q3.*rk.*t2.*t7.*t8.*t18.*4.8e1-COM_X.*Jw.*Mb.*q1.*q3.*rk.*t3.*t7.*t8.*t18.*2.4e2-COM_Y.*Jw.*Mb.*q1.*q2.*rk.*t4.*t7.*t8.*t18.*4.8e2-COM_X.*Jw.*Mb.*q1.*q3.*rk.*t2.*t8.*t11.*t18.*1.44e2-COM_X.*Jw.*Mb.*q1.*q3.*rk.*t4.*t8.*t9.*t18.*1.44e2-COM_X.*Jw.*Mb.*q2.*q4.*rk.*t4.*t7.*t8.*t18.*4.8e2-COM_X.*Jw.*Mb.*q1.*q3.*rk.*t2.*t8.*t12.*t18.*2.4e2-COM_X.*Jw.*Mb.*q1.*q3.*rk.*t3.*t8.*t11.*t18.*2.4e2-COM_X.*Jw.*Mb.*q1.*q3.*rk.*t4.*t8.*t10.*t18.*2.4e2-COM_X.*Jw.*Mb.*q1.*q3.*rk.*t3.*t8.*t12.*t18.*1.44e2-COM_Y.*Jw.*Mb.*q1.*q2.*rk.*t4.*t8.*t11.*t18.*4.8e2-COM_Y.*Jw.*Mb.*q3.*q4.*rk.*t4.*t7.*t8.*t18.*4.8e2-COM_X.*Jw.*Mb.*q1.*q3.*rk.*t7.*t8.*t9.*t18.*2.4e2-COM_Y.*Jw.*Mb.*q1.*q2.*rk.*t4.*t8.*t12.*t18.*2.88e2-COM_X.*Jw.*Mb.*q1.*q3.*rk.*t7.*t8.*t10.*t18.*1.44e2-COM_X.*Jw.*Mb.*q2.*q4.*rk.*t4.*t8.*t11.*t18.*4.8e2-COM_X.*Jw.*Mb.*q2.*q4.*rk.*t4.*t8.*t12.*t18.*2.88e2-COM_Y.*Jw.*Mb.*q1.*q2.*rk.*t7.*t8.*t11.*t18.*2.88e2-COM_Y.*Jw.*Mb.*q1.*q2.*rk.*t7.*t8.*t12.*t18.*4.8e2-COM_Y.*Jw.*Mb.*q3.*q4.*rk.*t4.*t8.*t11.*t18.*4.8e2-COM_X.*Jw.*Mb.*q1.*q3.*rk.*t8.*t9.*t11.*t18.*2.4e2-COM_X.*Jw.*Mb.*q2.*q4.*rk.*t7.*t8.*t11.*t18.*2.88e2-COM_Y.*Jw.*Mb.*q3.*q4.*rk.*t4.*t8.*t12.*t18.*2.88e2-COM_X.*Jw.*Mb.*q1.*q3.*rk.*t8.*t9.*t12.*t18.*4.8e1-COM_X.*Jw.*Mb.*q1.*q3.*rk.*t8.*t10.*t11.*t18.*4.8e1-COM_X.*Jw.*Mb.*q2.*q4.*rk.*t7.*t8.*t12.*t18.*4.8e2-COM_X.*Jw.*Mb.*q1.*q3.*rk.*t8.*t10.*t12.*t18.*2.4e2-COM_Y.*Jw.*Mb.*q3.*q4.*rk.*t7.*t8.*t11.*t18.*2.88e2-COM_Y.*Jw.*Mb.*q1.*q2.*rk.*t8.*t11.*t12.*t18.*4.8e2-COM_Y.*Jw.*Mb.*q3.*q4.*rk.*t7.*t8.*t12.*t18.*4.8e2-COM_X.*Jw.*Mb.*q2.*q4.*rk.*t8.*t11.*t12.*t18.*4.8e2-COM_Y.*Jw.*Mb.*q3.*q4.*rk.*t8.*t11.*t12.*t18.*4.8e2-COM_Z.*Jw.*Mb.*rk.*t2.*t3.*t7.*t8.*t9.*t18.*1.92e2-COM_Z.*Jw.*Mb.*rk.*t2.*t3.*t7.*t8.*t10.*t18.*4.8e2-COM_Z.*Jw.*Mb.*rk.*t2.*t3.*t8.*t9.*t11.*t18.*5.76e2-COM_Z.*Jw.*Mb.*rk.*t2.*t3.*t8.*t10.*t11.*t18.*4.8e2-COM_Z.*Jw.*Mb.*rk.*t2.*t7.*t8.*t9.*t10.*t18.*4.8e2-COM_Z.*Jw.*Mb.*rk.*t3.*t7.*t8.*t9.*t10.*t18.*5.76e2-COM_Z.*Jw.*Mb.*rk.*t2.*t8.*t9.*t10.*t11.*t18.*4.8e2-COM_Z.*Jw.*Mb.*rk.*t3.*t8.*t9.*t10.*t11.*t18.*1.92e2-COM_X.*COM_Y.*q2.*q3.*t2.*t3.*t8.*t9.*t19.*t20.*2.56e2-COM_Y.*COM_Z.*q1.*q2.*t2.*t3.*t8.*t9.*t19.*t20.*1.28e2-COM_X.*COM_Y.*q1.*q4.*t2.*t3.*t8.*t10.*t19.*t20.*2.56e2-COM_X.*COM_Z.*q1.*q3.*t2.*t3.*t8.*t10.*t19.*t20.*1.28e2-COM_X.*COM_Z.*q2.*q4.*t2.*t3.*t8.*t9.*t19.*t20.*1.28e2-COM_Y.*COM_Z.*q3.*q4.*t2.*t3.*t8.*t9.*t19.*t20.*1.28e2-COM_X.*COM_Z.*q1.*q3.*t2.*t8.*t9.*t10.*t19.*t20.*1.28e2-COM_Y.*COM_Z.*q1.*q2.*t3.*t8.*t9.*t10.*t19.*t20.*1.28e2-COM_X.*COM_Z.*q2.*q4.*t3.*t8.*t9.*t10.*t19.*t20.*1.28e2-COM_Y.*COM_Z.*q3.*q4.*t3.*t8.*t9.*t10.*t19.*t20.*1.28e2-q1.*q2.*q3.*q4.*t2.*t3.*t8.*t19.*t20.*t21.*2.56e2-q1.*q2.*q3.*q4.*t2.*t8.*t9.*t19.*t20.*t21.*2.56e2-q1.*q2.*q3.*q4.*t2.*t8.*t10.*t19.*t20.*t21.*2.56e2-q1.*q2.*q3.*q4.*t3.*t8.*t9.*t19.*t20.*t21.*2.56e2-q1.*q2.*q3.*q4.*t3.*t8.*t10.*t19.*t20.*t21.*2.56e2-q1.*q2.*q3.*q4.*t8.*t9.*t10.*t19.*t20.*t21.*2.56e2-COM_Y.*Jw.*Mb.*q1.*q2.*rk.*t2.*t3.*t4.*t8.*t18.*2.88e2-COM_X.*Jw.*Mb.*q2.*q4.*rk.*t2.*t3.*t4.*t8.*t18.*2.88e2-COM_Y.*Jw.*Mb.*q1.*q2.*rk.*t2.*t3.*t7.*t8.*t18.*2.88e2-COM_Y.*Jw.*Mb.*q3.*q4.*rk.*t2.*t3.*t4.*t8.*t18.*2.88e2-COM_X.*Jw.*Mb.*q1.*q3.*rk.*t2.*t3.*t8.*t9.*t18.*4.8e2-COM_X.*Jw.*Mb.*q2.*q4.*rk.*t2.*t3.*t7.*t8.*t18.*2.88e2-COM_X.*Jw.*Mb.*q1.*q3.*rk.*t2.*t3.*t8.*t10.*t18.*4.8e2-COM_Y.*Jw.*Mb.*q1.*q2.*rk.*t2.*t4.*t8.*t9.*t18.*8.64e2-COM_Y.*Jw.*Mb.*q1.*q2.*rk.*t2.*t3.*t8.*t11.*t18.*8.64e2-COM_Y.*Jw.*Mb.*q1.*q2.*rk.*t2.*t4.*t8.*t10.*t18.*9.6e2-COM_Y.*Jw.*Mb.*q1.*q2.*rk.*t3.*t4.*t8.*t9.*t18.*1.92e2-COM_Y.*Jw.*Mb.*q3.*q4.*rk.*t2.*t3.*t7.*t8.*t18.*2.88e2-COM_X.*Jw.*Mb.*q2.*q4.*rk.*t2.*t4.*t8.*t9.*t18.*8.64e2-COM_Y.*Jw.*Mb.*q1.*q2.*rk.*t2.*t3.*t8.*t12.*t18.*8.64e2-COM_Y.*Jw.*Mb.*q1.*q2.*rk.*t3.*t4.*t8.*t10.*t18.*2.88e2-COM_X.*Jw.*Mb.*q2.*q4.*rk.*t2.*t3.*t8.*t11.*t18.*8.64e2-COM_X.*Jw.*Mb.*q2.*q4.*rk.*t2.*t4.*t8.*t10.*t18.*9.6e2-COM_X.*Jw.*Mb.*q2.*q4.*rk.*t3.*t4.*t8.*t9.*t18.*1.92e2-COM_Y.*Jw.*Mb.*q1.*q2.*rk.*t2.*t7.*t8.*t9.*t18.*2.88e2-COM_X.*Jw.*Mb.*q2.*q4.*rk.*t2.*t3.*t8.*t12.*t18.*8.64e2-COM_X.*Jw.*Mb.*q2.*q4.*rk.*t3.*t4.*t8.*t10.*t18.*2.88e2-COM_Y.*Jw.*Mb.*q1.*q2.*rk.*t2.*t7.*t8.*t10.*t18.*1.92e2-COM_Y.*Jw.*Mb.*q1.*q2.*rk.*t3.*t7.*t8.*t9.*t18.*9.6e2-COM_Y.*Jw.*Mb.*q3.*q4.*rk.*t2.*t4.*t8.*t9.*t18.*8.64e2-COM_X.*Jw.*Mb.*q2.*q4.*rk.*t2.*t7.*t8.*t9.*t18.*2.88e2-COM_Y.*Jw.*Mb.*q1.*q2.*rk.*t3.*t7.*t8.*t10.*t18.*8.64e2-COM_Y.*Jw.*Mb.*q3.*q4.*rk.*t2.*t3.*t8.*t11.*t18.*8.64e2-COM_Y.*Jw.*Mb.*q3.*q4.*rk.*t2.*t4.*t8.*t10.*t18.*9.6e2-COM_Y.*Jw.*Mb.*q3.*q4.*rk.*t3.*t4.*t8.*t9.*t18.*1.92e2-COM_X.*Jw.*Mb.*q1.*q3.*rk.*t2.*t8.*t9.*t10.*t18.*4.8e2-COM_X.*Jw.*Mb.*q2.*q4.*rk.*t2.*t7.*t8.*t10.*t18.*1.92e2-COM_X.*Jw.*Mb.*q2.*q4.*rk.*t3.*t7.*t8.*t9.*t18.*9.6e2-COM_Y.*Jw.*Mb.*q3.*q4.*rk.*t2.*t3.*t8.*t12.*t18.*8.64e2-COM_Y.*Jw.*Mb.*q3.*q4.*rk.*t3.*t4.*t8.*t10.*t18.*2.88e2-COM_X.*Jw.*Mb.*q1.*q3.*rk.*t3.*t8.*t9.*t10.*t18.*4.8e2-COM_X.*Jw.*Mb.*q2.*q4.*rk.*t3.*t7.*t8.*t10.*t18.*8.64e2-COM_Y.*Jw.*Mb.*q1.*q2.*rk.*t2.*t8.*t9.*t11.*t18.*8.64e2-COM_Y.*Jw.*Mb.*q3.*q4.*rk.*t2.*t7.*t8.*t9.*t18.*2.88e2-COM_Y.*Jw.*Mb.*q1.*q2.*rk.*t2.*t8.*t9.*t12.*t18.*2.88e2-COM_Y.*Jw.*Mb.*q1.*q2.*rk.*t2.*t8.*t10.*t11.*t18.*1.92e2-COM_Y.*Jw.*Mb.*q1.*q2.*rk.*t3.*t8.*t9.*t11.*t18.*9.6e2-COM_Y.*Jw.*Mb.*q1.*q2.*rk.*t4.*t8.*t9.*t10.*t18.*8.64e2-COM_Y.*Jw.*Mb.*q3.*q4.*rk.*t2.*t7.*t8.*t10.*t18.*1.92e2-COM_Y.*Jw.*Mb.*q3.*q4.*rk.*t3.*t7.*t8.*t9.*t18.*9.6e2-COM_X.*Jw.*Mb.*q2.*q4.*rk.*t2.*t8.*t9.*t11.*t18.*8.64e2-COM_Y.*Jw.*Mb.*q1.*q2.*rk.*t2.*t8.*t10.*t12.*t18.*9.6e2-COM_Y.*Jw.*Mb.*q1.*q2.*rk.*t3.*t8.*t9.*t12.*t18.*1.92e2-COM_Y.*Jw.*Mb.*q1.*q2.*rk.*t3.*t8.*t10.*t11.*t18.*2.88e2-COM_Y.*Jw.*Mb.*q3.*q4.*rk.*t3.*t7.*t8.*t10.*t18.*8.64e2-COM_X.*Jw.*Mb.*q2.*q4.*rk.*t2.*t8.*t9.*t12.*t18.*2.88e2-COM_X.*Jw.*Mb.*q2.*q4.*rk.*t2.*t8.*t10.*t11.*t18.*1.92e2-COM_X.*Jw.*Mb.*q2.*q4.*rk.*t3.*t8.*t9.*t11.*t18.*9.6e2-COM_X.*Jw.*Mb.*q2.*q4.*rk.*t4.*t8.*t9.*t10.*t18.*8.64e2-COM_Y.*Jw.*Mb.*q1.*q2.*rk.*t3.*t8.*t10.*t12.*t18.*8.64e2-COM_X.*Jw.*Mb.*q2.*q4.*rk.*t2.*t8.*t10.*t12.*t18.*9.6e2-COM_X.*Jw.*Mb.*q2.*q4.*rk.*t3.*t8.*t9.*t12.*t18.*1.92e2-COM_X.*Jw.*Mb.*q2.*q4.*rk.*t3.*t8.*t10.*t11.*t18.*2.88e2-COM_Y.*Jw.*Mb.*q1.*q2.*rk.*t7.*t8.*t9.*t10.*t18.*8.64e2-COM_Y.*Jw.*Mb.*q3.*q4.*rk.*t2.*t8.*t9.*t11.*t18.*8.64e2-COM_X.*Jw.*Mb.*q2.*q4.*rk.*t3.*t8.*t10.*t12.*t18.*8.64e2-COM_Y.*Jw.*Mb.*q3.*q4.*rk.*t2.*t8.*t9.*t12.*t18.*2.88e2-COM_Y.*Jw.*Mb.*q3.*q4.*rk.*t2.*t8.*t10.*t11.*t18.*1.92e2-COM_Y.*Jw.*Mb.*q3.*q4.*rk.*t3.*t8.*t9.*t11.*t18.*9.6e2-COM_Y.*Jw.*Mb.*q3.*q4.*rk.*t4.*t8.*t9.*t10.*t18.*8.64e2-COM_X.*Jw.*Mb.*q2.*q4.*rk.*t7.*t8.*t9.*t10.*t18.*8.64e2-COM_Y.*Jw.*Mb.*q3.*q4.*rk.*t2.*t8.*t10.*t12.*t18.*9.6e2-COM_Y.*Jw.*Mb.*q3.*q4.*rk.*t3.*t8.*t9.*t12.*t18.*1.92e2-COM_Y.*Jw.*Mb.*q3.*q4.*rk.*t3.*t8.*t10.*t11.*t18.*2.88e2-COM_Y.*Jw.*Mb.*q3.*q4.*rk.*t3.*t8.*t10.*t12.*t18.*8.64e2-COM_Y.*Jw.*Mb.*q1.*q2.*rk.*t8.*t9.*t10.*t11.*t18.*2.88e2-COM_Y.*Jw.*Mb.*q3.*q4.*rk.*t7.*t8.*t9.*t10.*t18.*8.64e2-COM_Y.*Jw.*Mb.*q1.*q2.*rk.*t8.*t9.*t10.*t12.*t18.*2.88e2-COM_X.*Jw.*Mb.*q2.*q4.*rk.*t8.*t9.*t10.*t11.*t18.*2.88e2-COM_X.*Jw.*Mb.*q2.*q4.*rk.*t8.*t9.*t10.*t12.*t18.*2.88e2-COM_Y.*Jw.*Mb.*q3.*q4.*rk.*t8.*t9.*t10.*t11.*t18.*2.88e2-COM_Y.*Jw.*Mb.*q3.*q4.*rk.*t8.*t9.*t10.*t12.*t18.*2.88e2-COM_Y.*Jw.*Mb.*q1.*q2.*rk.*t2.*t3.*t8.*t9.*t10.*t18.*1.92e3-COM_X.*Jw.*Mb.*q2.*q4.*rk.*t2.*t3.*t8.*t9.*t10.*t18.*1.92e3-COM_Y.*Jw.*Mb.*q3.*q4.*rk.*t2.*t3.*t8.*t9.*t10.*t18.*1.92e3; 
+  // 'SteadyStateAcceleration:482' t476 = 1.0./t475;
+  f11 = (((((((((((((((((((((((((((((((t20 * (Jk * Jk) * 16.0F + t5 * t14 * t24 *
+    9.0F) + t5 * t16 * t24 * 9.0F) + t5 * t13 * t24 * 9.0F) + t5 * t15 * t24 *
+    9.0F) + t5 * t7 * t14 * t24 * 9.0F) + t5 * t12 * t16 * t24 * 9.0F) + t4 * t5
+    * t13 * t24 * 9.0F) + t5 * t11 * t15 * t24 * 9.0F) + t19 * t20 * t24 * 16.0F)
+    + t20 * t24 * (Mk * Mk) * 16.0F) + t2 * t5 * t9 * t12 * t24 * 72.0F) + t5 *
+    t7 * t12 * t24 * 126.0F) + t2 * t5 * t7 * t9 * t24 * 72.0F) + t2 * t3 * t4 *
+    t5 * t24 * 72.0F) + t4 * t5 * t7 * t24 * 126.0F) + t2 * t3 * t5 * t7 * t24 *
+                        72.0F) + t2 * t5 * t10 * t11 * t24 * 36.0F) + t3 * t4 *
+                      t5 * t9 * t24 * 36.0F) + t5 * t7 * t11 * t24 * 54.0F) + t4
+                    * t5 * t12 * t24 * 54.0F) + t2 * t5 * t7 * t10 * t24 * 36.0F)
+                  + t3 * t5 * t9 * t12 * t24 * 36.0F) + t5 * t9 * t10 * t11 *
+                 t24 * 72.0F) + t5 * t11 * t12 * t24 * 126.0F) + t5 * t9 * t10 *
+               t12 * t24 * 72.0F) + t2 * t5 * t9 * t16 * t24 * 90.0F) + t5 * t7 *
+             t16 * t24 * 279.0F) + t3 * t5 * t10 * t11 * t24 * 72.0F) + t2 * t5 *
+           t7 * t9 * t12 * t24 * 396.0F) + t4 * t5 * t11 * t24 * 126.0F) + t5 *
+         t12 * t14 * t24 * 279.0F) + t3 * t4 * t5 * t10 * t24 * 72.0F;
+  f12 = t2 * t5 * t9 * t14 * t24 * 90.0F;
+  f13 = t2 * t3 * t5 * t13 * t24;
+  f14 = t5 * t7 * t13 * t24;
+  f15 = t2 * t3 * t4 * t5 * t7;
+  f16 = t4 * t5 * t14;
+  f17 = t2 * t3 * t5;
+  f18 = t2 * t5 * t10;
+  f19 = t3 * t5;
+  f20 = t2 * t5;
+  f11 = ((((((((((((((((((((((((((((((((((((((((((((((((f11 + f12) + f13 * 90.0F)
+    + f14 * 279.0F) + f15 * t24 * 396.0F) + f16 * t24 * 279.0F) + f17 * t14 *
+    t24 * 90.0F) + f18 * t15 * t24 * 54.0F) + f19 * t9 * t13 * t24 * 54.0F) + t5
+    * t7 * t15 * t24 * 135.0F) + t5 * t12 * t13 * t24 * 135.0F) + f20 * t7 * t10
+    * t11 * t24 * 180.0F) + t3 * t4 * t5 * t9 * t12 * t24 * 180.0F) + t5 * t11 *
+    t14 * t24 * 135.0F) + t4 * t5 * t16 * t24 * 135.0F) + t2 * t5 * t10 * t14 *
+    t24 * 54.0F) + t3 * t5 * t9 * t16 * t24 * 54.0F) + t5 * t9 * t10 * t15 * t24
+    * 90.0F) + t5 * t12 * t15 * t24 * 279.0F) + t5 * t9 * t10 * t11 * t12 * t24 *
+    396.0F) + t5 * t11 * t16 * t24 * 279.0F) + t5 * t9 * t10 * t16 * t24 * 90.0F)
+    + t3 * t5 * t10 * t15 * t24 * 90.0F) + t4 * t5 * t15 * t24 * 279.0F) + t3 *
+    t4 * t5 * t10 * t11 * t24 * 396.0F) + t5 * t11 * t13 * t24 * 279.0F) + t3 *
+    t5 * t10 * t13 * t24 * 90.0F) + Jk * Mb * t8 * t20 * 32.0F) + Jk * Mk * t8 *
+    t20 * 32.0F) + Mb * Mk * t20 * t24 * 32.0F) + t8 * t14 * t19 * t20 * t22 *
+    16.0F) + t8 * t16 * t19 * t20 * t22 * 16.0F) + t8 * t13 * t19 * t20 * t22 *
+    16.0F) + t8 * t15 * t19 * t20 * t22 * 16.0F) + t2 * t4 * t5 * t9 * t24 *
+                       216.0F) + t2 * t3 * t5 * t12 * t24 * 216.0F) + t3 * t5 *
+                     t7 * t9 * t24 * 252.0F) + t2 * t5 * t9 * t11 * t24 * 216.0F)
+                   + t2 * t5 * t10 * t12 * t24 * 252.0F) + t5 * t7 * t9 * t10 *
+                  t24 * 216.0F) + t2 * t3 * t5 * t11 * t24 * 216.0F) + t2 * t4 *
+                t5 * t10 * t24 * 252.0F) + t3 * t5 * t7 * t10 * t24 * 216.0F) +
+              t3 * t5 * t9 * t11 * t24 * 252.0F) + t4 * t5 * t9 * t10 * t24 *
+             216.0F) + t3 * t5 * t10 * t12 * t24 * 216.0F) + t2 * t5 * t9 * t13 *
+           t24 * 450.0F) + t2 * t3 * t4 * t5 * t12 * t24 * 900.0F) + t2 * t4 *
+         t5 * t9 * t12 * t24 * 900.0F) + t2 * t3 * t5 * t16 * t24 * 450.0F;
+  f12 = t3 * t4 * t5 * t7 * t9 * t24 * 1116.0F;
+  f13 = t4 * t5 * t7 * t12 * t24 * 1674.0F;
+  f14 = t3 * t5 * t7 * t9 * t12 * t24;
+  f15 = t2 * t4 * t5 * t7 * t9 * t24;
+  f16 = t2 * t3 * t5 * t7 * t12;
+  f17 = t3 * t5 * t9 * t14;
+  f18 = t2 * t5 * t9;
+  f19 = t2 * t5 * t10 * t11;
+  f20 = t2 * t5 * t9;
+  f21 = t2 * t5;
+  f22 = t5 * t7;
+  f11 = (((((((((((((((((((((((((((((((((((((((((((((((((f11 + f12) + f13) + f14
+    * 1116.0F) + f15 * 1188.0F) + f16 * t24 * 1188.0F) + f17 * t24 * 558.0F) +
+    f18 * t15 * t24 * 450.0F) + f19 * t12 * t24 * 1116.0F) + f20 * t11 * t12 *
+    t24 * 1188.0F) + f21 * t10 * t16 * t24 * 558.0F) + f22 * t9 * t10 * t11 *
+    t24 * 900.0F) + t5 * t7 * t11 * t12 * t24 * 1674.0F) + t5 * t7 * t9 * t10 *
+    t12 * t24 * 1188.0F) + t2 * t5 * t7 * t9 * t11 * t24 * 900.0F) + t2 * t5 *
+    t7 * t10 * t12 * t24 * 1116.0F) + t5 * t9 * t10 * t14 * t24 * 450.0F) + t2 *
+    t3 * t5 * t15 * t24 * 450.0F) + t2 * t4 * t5 * t10 * t11 * t24 * 1116.0F) +
+    t2 * t3 * t4 * t5 * t11 * t24 * 1188.0F) + t2 * t5 * t10 * t13 * t24 *
+    558.0F) + t3 * t5 * t7 * t10 * t11 * t24 * 900.0F) + t4 * t5 * t7 * t11 *
+    t24 * 1674.0F) + t3 * t4 * t5 * t7 * t10 * t24 * 1188.0F) + t2 * t3 * t5 *
+    t7 * t11 * t24 * 900.0F) + t2 * t4 * t5 * t7 * t10 * t24 * 1116.0F) + t3 *
+    t5 * t10 * t14 * t24 * 450.0F) + t3 * t5 * t9 * t15 * t24 * 558.0F) + t4 *
+    t5 * t9 * t10 * t11 * t24 * 1188.0F) + t3 * t4 * t5 * t9 * t11 * t24 *
+    1116.0F) + t5 * t9 * t10 * t13 * t24 * 450.0F) + t3 * t5 * t10 * t11 * t12 *
+    t24 * 1188.0F) + t4 * t5 * t11 * t12 * t24 * 1674.0F) + t3 * t4 * t5 * t10 *
+    t12 * t24 * 900.0F) + t3 * t5 * t9 * t11 * t12 * t24 * 1116.0F) + t4 * t5 *
+                       t9 * t10 * t12 * t24 * 900.0F) + t3 * t5 * t10 * t16 *
+                      t24 * 450.0F) + Jk * Jw * t7 * t8 * t18 * 24.0F) + Jk * Jw
+                    * t8 * t12 * t18 * 24.0F) + Jk * Jw * t4 * t8 * t18 * 24.0F)
+                  + Jk * Jw * t8 * t11 * t18 * 24.0F) + Jw * Mb * t7 * t18 * t24
+                 * 24.0F) + Jw * Mb * t12 * t18 * t24 * 24.0F) + Jw * Mb * t4 *
+               t18 * t24 * 24.0F) + Jw * Mb * t11 * t18 * t24 * 24.0F) + Jw * Mk
+             * t7 * t18 * t24 * 24.0F) + Jw * Mk * t12 * t18 * t24 * 24.0F) + Jw
+           * Mk * t4 * t18 * t24 * 24.0F) + Jw * Mk * t11 * t18 * t24 * 24.0F) +
+         COM_Z * rk * t7 * t8 * t19 * t20 * 32.0F) + COM_Z * rk * t8 * t11 * t19
+    * t20 * 32.0F;
+  f12 = t2 * t3 * t5 * t9 * t10 * t24 * 504.0F;
+  f13 = t2 * t3 * t5 * t9 * t10 * t11 * t24;
+  f14 = t2 * t4 * t5 * t9 * t11 * t24;
+  f15 = t2 * t3 * t4 * t5 * t9 * t10;
+  f16 = t2 * t3 * t5 * t11 * t12;
+  f17 = t2 * t4 * t5 * t10;
+  f18 = t2 * t3 * t5 * t9 * t10;
+  f19 = t3 * t5 * t7;
+  f20 = t4 * t5 * t7;
+  f21 = t3 * t5;
+  f22 = t2 * t3 * t5;
+  f23 = COM_Y * q1 * q2;
+  f24 = COM_Y * q1 * q2;
+  f25 = COM_X * q2;
+  f26 = COM_X * q2;
+  f11 = ((((((((((((((((((((((((((((((((((((((((((((((((f11 + f12) + f13 *
+    2232.0F) + f14 * 3564.0F) + f15 * t24 * 2232.0F) + f16 * t24 * 3564.0F) +
+    f17 * t12 * t24 * 3348.0F) + f18 * t12 * t24 * 2232.0F) + f19 * t9 * t11 *
+    t24 * 3348.0F) + f20 * t9 * t10 * t24 * 3564.0F) + f21 * t7 * t10 * t12 *
+    t24 * 3564.0F) + f22 * t7 * t9 * t10 * t24 * 2232.0F) + f23 * rk * t8 * t9 *
+    t19 * t20 * 64.0F) + f24 * rk * t2 * t8 * t19 * t20 * 64.0F) + f25 * q4 * rk
+    * t8 * t10 * t19 * t20 * 64.0F) + f26 * q4 * rk * t8 * t9 * t19 * t20 *
+    64.0F) + COM_Y * q3 * q4 * rk * t8 * t10 * t19 * t20 * 64.0F) + COM_Y * q3 *
+    q4 * rk * t3 * t8 * t19 * t20 * 64.0F) + Jk * Jw * t2 * t8 * t9 * t18 *
+    96.0F) + Jk * Jw * t2 * t3 * t8 * t18 * 96.0F) + Jk * Jw * t2 * t8 * t10 *
+    t18 * 48.0F) + Jk * Jw * t3 * t8 * t9 * t18 * 48.0F) + Jk * Jw * t8 * t9 *
+    t10 * t18 * 96.0F) + Jk * Jw * t3 * t8 * t10 * t18 * 96.0F) + Jw * Mb * t2 *
+    t9 * t18 * t24 * 96.0F) + Jw * Mb * t2 * t3 * t18 * t24 * 96.0F) + Jw * Mb *
+    t2 * t10 * t18 * t24 * 48.0F) + Jw * Mb * t3 * t9 * t18 * t24 * 48.0F) + Jw *
+    Mb * t9 * t10 * t18 * t24 * 96.0F) + Jw * Mb * t3 * t10 * t18 * t24 * 96.0F)
+    + Jw * Mk * t2 * t9 * t18 * t24 * 96.0F) + Jw * Mk * t2 * t3 * t18 * t24 *
+    96.0F) + Jw * Mk * t2 * t10 * t18 * t24 * 48.0F) + Jw * Mk * t3 * t9 * t18 *
+                        t24 * 48.0F) + Jw * Mk * t9 * t10 * t18 * t24 * 96.0F) +
+                      Jw * Mk * t3 * t10 * t18 * t24 * 96.0F) + COM_Z * Jk * Mb *
+                     rk * t7 * t20 * 32.0F) + COM_Z * Jk * Mb * rk * t11 * t20 *
+                    32.0F) + COM_Z * rk * t2 * t8 * t10 * t19 * t20 * 64.0F) +
+                  COM_Z * Jw * Mb * rk * t8 * t14 * t18 * 24.0F) + COM_Z * Jw *
+                 Mb * rk * t8 * t9 * t16 * t18 * 24.0F) + COM_Z * Jw * Mb * rk *
+                t8 * t15 * t18 * 24.0F) + COM_Z * Jw * Mb * rk * t3 * t8 * t13 *
+               t18 * 24.0F) + COM_Z * Mb * Mk * rk * t7 * t8 * t20 * 32.0F) +
+             COM_Z * Mb * Mk * rk * t8 * t11 * t20 * 32.0F) + t2 * t3 * t4 * t8 *
+            t19 * t20 * t21 * 64.0F) + t4 * t7 * t8 * t19 * t20 * t21 * 128.0F)
+          + t2 * t3 * t7 * t8 * t19 * t20 * t21 * 64.0F) + t2 * t8 * t9 * t12 *
+         t19 * t20 * t23 * 64.0F) + t7 * t8 * t12 * t19 * t20 * t23 * 128.0F;
+  f12 = t2 * t7 * t8 * t9 * t19 * t20 * t23 * 64.0F;
+  f13 = t8 * t9 * t10 * t11 * t19 * t20 * t21 * 64.0F;
+  f14 = t8 * t11 * t12 * t19 * t20 * t21;
+  f15 = t8 * t9 * t10 * t12 * t19 * t20 * t21;
+  f16 = t2 * t8 * t10 * t11 * t19 * t20;
+  f17 = t3 * t4 * t8 * t9 * t19 * t20;
+  f18 = t7 * t8 * t11 * t19;
+  f19 = t4 * t8 * t12 * t19;
+  f20 = t2 * t7 * t8 * t10;
+  f21 = t3 * t8 * t9 * t12;
+  f22 = t3 * t8 * t10;
+  f23 = t4 * t8;
+  f24 = t3 * t4;
+  f25 = COM_Z * Jk;
+  f26 = COM_Y * Jw * Mb * q1;
+  f27 = COM_Y * Jw * Mb * q1;
+  f28 = COM_X * Jw;
+  f29 = COM_X * Jw * Mb;
+  f30 = COM_X * Jw;
+  f11 = (((((((((((((((((((((((((((((((((((((((((((((((((f11 + f12) + f13) + f14
+    * 128.0F) + f15 * 64.0F) + f16 * t22 * 64.0F) + f17 * t22 * 64.0F) + f18 *
+    t20 * t22 * 96.0F) + f19 * t20 * t22 * 96.0F) + f20 * t19 * t20 * t22 *
+    64.0F) + f21 * t19 * t20 * t22 * 64.0F) + f22 * t11 * t19 * t20 * t23 *
+    64.0F) + f23 * t11 * t19 * t20 * t23 * 128.0F) + f24 * t8 * t10 * t19 * t20 *
+    t23 * 64.0F) + f25 * Mb * rk * t2 * t10 * t20 * 64.0F) + f26 * q2 * rk * t8 *
+    t9 * t12 * t18 * 48.0F) + f27 * q2 * rk * t2 * t7 * t8 * t18 * 48.0F) + f28 *
+    Mb * q1 * q3 * rk * t8 * t13 * t18 * 48.0F) + f29 * q2 * q4 * rk * t8 * t10 *
+    t11 * t18 * 48.0F) + f30 * Mb * q2 * q4 * rk * t8 * t9 * t12 * t18 * 48.0F)
+    + COM_X * Jw * Mb * q1 * q3 * rk * t8 * t14 * t18 * 48.0F) + COM_Y * Jw * Mb
+    * q3 * q4 * rk * t8 * t10 * t11 * t18 * 48.0F) + COM_Y * Jw * Mb * q3 * q4 *
+    rk * t3 * t4 * t8 * t18 * 48.0F) + COM_Y * Mb * Mk * q1 * q2 * rk * t8 * t9 *
+    t20 * 64.0F) + COM_Y * Mb * Mk * q1 * q2 * rk * t2 * t8 * t20 * 64.0F) +
+    COM_X * Mb * Mk * q2 * q4 * rk * t8 * t10 * t20 * 64.0F) + COM_X * Mb * Mk *
+    q2 * q4 * rk * t8 * t9 * t20 * 64.0F) + COM_Y * Mb * Mk * q3 * q4 * rk * t8 *
+    t10 * t20 * 64.0F) + COM_Y * Mb * Mk * q3 * q4 * rk * t3 * t8 * t20 * 64.0F)
+    + COM_X * COM_Z * q1 * q3 * t3 * t4 * t8 * t19 * t20 * 64.0F) + COM_Y *
+    COM_Z * q1 * q2 * t2 * t7 * t8 * t19 * t20 * 64.0F) + COM_X * COM_Z * q2 *
+    q4 * t8 * t10 * t11 * t19 * t20 * 64.0F) + COM_Y * COM_Z * q3 * q4 * t8 *
+    t10 * t11 * t19 * t20 * 64.0F) + COM_Y * Jw * Mb * q1 * q2 * rk * t2 * t8 *
+    t12 * t18 * 240.0F) + COM_Y * Jw * Mb * q1 * q2 * rk * t7 * t8 * t9 * t18 *
+                        240.0F) + COM_Z * Jw * Mb * rk * t2 * t7 * t8 * t9 * t18
+                       * 96.0F) + COM_X * Jw * Mb * q1 * q3 * rk * t2 * t3 * t4 *
+                      t8 * t18 * 288.0F) + COM_X * Jw * Mb * q2 * q4 * rk * t8 *
+                     t9 * t11 * t18 * 240.0F) + COM_X * Jw * Mb * q1 * q3 * rk *
+                    t4 * t7 * t8 * t18 * 480.0F) + COM_X * Jw * Mb * q2 * q4 *
+                   rk * t8 * t10 * t12 * t18 * 240.0F) + COM_X * Jw * Mb * q1 *
+                  q3 * rk * t2 * t3 * t7 * t8 * t18 * 288.0F) + COM_Z * Jw * Mb *
+                 rk * t2 * t3 * t7 * t8 * t18 * 96.0F) + COM_Z * Jw * Mb * rk *
+                t2 * t8 * t16 * t18 * 120.0F) + COM_Z * Jw * Mb * rk * t2 * t8 *
+               t10 * t11 * t18 * 96.0F) + COM_Z * Jw * Mb * rk * t7 * t8 * t9 *
+              t12 * t18 * 96.0F) + COM_Z * Jw * Mb * rk * t7 * t8 * t11 * t18 *
+             144.0F) + COM_Z * Jw * Mb * rk * t2 * t7 * t8 * t10 * t18 * 96.0F)
+           + COM_Y * Jw * Mb * q3 * q4 * rk * t3 * t8 * t11 * t18 * 240.0F) +
+          COM_Y * Jw * Mb * q3 * q4 * rk * t4 * t8 * t10 * t18 * 240.0F) + COM_Z
+         * Jw * Mb * rk * t2 * t8 * t13 * t18 * 120.0F) + COM_Z * Jw * Mb * rk *
+    t8 * t9 * t10 * t11 * t18 * 96.0F;
+  f12 = COM_Z * Jw * Mb * rk * t3 * t4 * t7 * t8 * t18 * 96.0F;
+  f13 = COM_Z * Jw * Mb * rk * t8 * t9 * t13 * t18;
+  f14 = COM_Z * Jw * Mb * rk * t3 * t8 * t10 * t11 * t18;
+  f15 = COM_Z * Jw * Mb * rk * t3 * t4 * t8 * t12;
+  f16 = COM_Z * Jw * Mb * rk * t4 * t8 * t9 * t12;
+  f17 = COM_Z * Jw * Mb * rk * t3 * t8;
+  f18 = COM_Z * Jw * Mb * rk * t8 * t9 * t11;
+  f19 = COM_Z * Jw * Mb * rk * t8;
+  f20 = COM_Z * Jw * Mb * rk * t3 * t4;
+  f21 = COM_Z * Jw * Mb * rk;
+  f22 = COM_Z * Mb * Mk * rk;
+  f23 = COM_X * q2 * q4;
+  f24 = COM_Y * q1 * q2;
+  f25 = COM_Y * q1;
+  f26 = COM_X * q2;
+  f11 = ((((((((((((((((((((((((((((((((((((((((((((((((f11 + f12) + f13 *
+    120.0F) + f14 * 96.0F) + f15 * t18 * 240.0F) + f16 * t18 * 240.0F) + f17 *
+    t16 * t18 * 120.0F) + f18 * t12 * t18 * 96.0F) + f19 * t10 * t16 * t18 *
+    120.0F) + f20 * t8 * t11 * t18 * 96.0F) + f21 * t8 * t10 * t13 * t18 *
+    120.0F) + f22 * t2 * t8 * t10 * t20 * 64.0F) + f23 * rk * t2 * t8 * t19 *
+    t20 * 64.0F) + f24 * rk * t3 * t8 * t19 * t20 * 64.0F) + f25 * q2 * rk * t8 *
+    t10 * t19 * t20 * 64.0F) + f26 * q4 * rk * t3 * t8 * t19 * t20 * 64.0F) +
+    COM_Y * q3 * q4 * rk * t2 * t8 * t19 * t20 * 64.0F) + COM_Y * q3 * q4 * rk *
+    t8 * t9 * t19 * t20 * 64.0F) + COM_Y * Jk * Mb * q1 * q2 * rk * t9 * t20 *
+    64.0F) + COM_Y * Jk * Mb * q1 * q2 * rk * t2 * t20 * 64.0F) + COM_X * Jk *
+    Mb * q2 * q4 * rk * t10 * t20 * 64.0F) + COM_X * Jk * Mb * q2 * q4 * rk * t9
+    * t20 * 64.0F) + COM_Y * Jk * Mb * q3 * q4 * rk * t10 * t20 * 64.0F) + COM_Y
+    * Jk * Mb * q3 * q4 * rk * t3 * t20 * 64.0F) + t2 * t4 * t8 * t9 * t19 * t20
+    * t21 * 128.0F) + t2 * t3 * t8 * t12 * t19 * t20 * t21 * 64.0F) + t3 * t7 *
+    t8 * t9 * t19 * t20 * t21 * 128.0F) + t2 * t8 * t9 * t11 * t19 * t20 * t21 *
+    128.0F) + t2 * t8 * t10 * t12 * t19 * t20 * t21 * 128.0F) + t7 * t8 * t9 *
+    t10 * t19 * t20 * t21 * 64.0F) + t2 * t4 * t8 * t9 * t19 * t20 * t23 * 64.0F)
+    + t2 * t3 * t8 * t12 * t19 * t20 * t23 * 128.0F) + t3 * t7 * t8 * t9 * t19 *
+    t20 * t23 * 128.0F) + t2 * t3 * t8 * t11 * t19 * t20 * t21 * 64.0F) + t2 *
+                       t4 * t8 * t10 * t19 * t20 * t21 * 128.0F) + t3 * t7 * t8 *
+                      t10 * t19 * t20 * t21 * 128.0F) + t2 * t8 * t9 * t11 * t19
+                     * t20 * t23 * 64.0F) + t2 * t8 * t10 * t12 * t19 * t20 *
+                    t23 * 128.0F) + t7 * t8 * t9 * t10 * t19 * t20 * t23 *
+                   128.0F) + t3 * t8 * t9 * t11 * t19 * t20 * t21 * 128.0F) + t4
+                 * t8 * t9 * t10 * t19 * t20 * t21 * 64.0F) + t3 * t8 * t10 *
+                t12 * t19 * t20 * t21 * 128.0F) + t2 * t3 * t8 * t11 * t19 * t20
+               * t23 * 128.0F) + t2 * t4 * t8 * t10 * t19 * t20 * t23 * 128.0F)
+             + t3 * t7 * t8 * t10 * t19 * t20 * t23 * 64.0F) + t3 * t8 * t9 *
+            t11 * t19 * t20 * t23 * 128.0F) + t4 * t8 * t9 * t10 * t19 * t20 *
+           t23 * 128.0F) + t3 * t8 * t10 * t12 * t19 * t20 * t23 * 64.0F) +
+         COM_X * COM_Z * q1 * q3 * t2 * t4 * t8 * t19 * t20 * 64.0F) + COM_Y *
+    COM_Z * q1 * q2 * t7 * t8 * t9 * t19 * t20 * 64.0F;
+  f12 = COM_X * COM_Z * q2 * q4 * t8 * t9 * t11 * t19 * t20 * 64.0F;
+  f13 = COM_Y * COM_Z * q3 * q4 * t3 * t8 * t11 * t19 * t20 * 64.0F;
+  f14 = COM_X * Jk * Mb * q2 * q4 * rk * t2 * t20;
+  f15 = COM_Y * Jk * Mb * q1 * q2 * rk * t3 * t20;
+  f16 = COM_Y * Jk * Mb * q1 * q2 * rk * t10;
+  f17 = COM_X * Jk * Mb * q2 * q4 * rk * t3;
+  f18 = COM_Y * Jk * Mb * q3 * q4 * rk;
+  f19 = COM_Y * Jk * Mb * q3 * q4 * rk;
+  f20 = COM_X * COM_Y * q1 * q4 * t8 * t9;
+  f21 = COM_X * COM_Y * q1 * q4 * t8 * t10;
+  f22 = COM_X * COM_Y * q1 * q4 * t2;
+  f23 = COM_X * COM_Y * q1 * q4 * t7;
+  f24 = COM_X * COM_Z * q1 * q3;
+  f25 = COM_X * COM_Z * q1 * q3;
+  f26 = COM_X * COM_Z * q1;
+  f27 = COM_X * COM_Z * q2;
+  f28 = COM_X * COM_Z;
+  f29 = COM_X * COM_Z;
+  f11 = (((((((((((((((((((((((((((((((((((((((((((((((((f11 + f12) + f13) + f14
+    * 64.0F) + f15 * 64.0F) + f16 * t20 * 64.0F) + f17 * t20 * 64.0F) + f18 * t2
+    * t20 * 64.0F) + f19 * t9 * t20 * 64.0F) + f20 * t11 * t19 * t20 * 128.0F) +
+    f21 * t12 * t19 * t20 * 256.0F) + f22 * t8 * t12 * t19 * t20 * 256.0F) + f23
+    * t8 * t9 * t19 * t20 * 128.0F) + f24 * t4 * t8 * t9 * t19 * t20 * 192.0F) +
+    f25 * t3 * t8 * t12 * t19 * t20 * 192.0F) + f26 * q3 * t2 * t8 * t12 * t19 *
+    t20 * 64.0F) + f27 * q4 * t2 * t8 * t11 * t19 * t20 * 192.0F) + f28 * q2 *
+    q4 * t7 * t8 * t10 * t19 * t20 * 192.0F) + f29 * q2 * q4 * t7 * t8 * t9 *
+    t19 * t20 * 64.0F) + COM_Y * COM_Z * q1 * q2 * t3 * t7 * t8 * t19 * t20 *
+    64.0F) + COM_X * COM_Y * q2 * q3 * t3 * t8 * t11 * t19 * t20 * 256.0F) +
+    COM_X * COM_Y * q2 * q3 * t4 * t8 * t10 * t19 * t20 * 128.0F) + COM_X *
+    COM_Y * q2 * q3 * t8 * t9 * t11 * t19 * t20 * 256.0F) + COM_X * COM_Y * q2 *
+    q3 * t8 * t10 * t12 * t19 * t20 * 128.0F) + COM_X * COM_Z * q1 * q3 * t4 *
+    t8 * t10 * t19 * t20 * 64.0F) + COM_Y * COM_Z * q1 * q2 * t8 * t9 * t11 *
+    t19 * t20 * 64.0F) + COM_Y * COM_Z * q1 * q2 * t2 * t8 * t11 * t19 * t20 *
+    192.0F) + COM_Y * COM_Z * q1 * q2 * t7 * t8 * t10 * t19 * t20 * 192.0F) +
+    COM_X * COM_Z * q2 * q4 * t3 * t8 * t11 * t19 * t20 * 64.0F) + COM_Y * COM_Z
+    * q3 * q4 * t2 * t8 * t11 * t19 * t20 * 192.0F) + COM_Y * COM_Z * q3 * q4 *
+    t7 * t8 * t10 * t19 * t20 * 192.0F) + COM_Y * COM_Z * q3 * q4 * t3 * t7 * t8
+    * t19 * t20 * 64.0F) + COM_Y * COM_Z * q3 * q4 * t8 * t9 * t11 * t19 * t20 *
+    64.0F) + COM_X * Jw * Mb * q2 * q4 * rk * t2 * t8 * t9 * t10 * t18 * 480.0F)
+                        + COM_Y * Jw * Mb * q1 * q2 * rk * t2 * t3 * t8 * t9 *
+                        t18 * 480.0F) + COM_X * Jw * Mb * q1 * q3 * rk * t2 * t4
+                       * t8 * t9 * t18 * 864.0F) + COM_X * Jw * Mb * q1 * q3 *
+                      rk * t2 * t3 * t8 * t12 * t18 * 864.0F) + COM_X * Jw * Mb *
+                     q1 * q3 * rk * t3 * t7 * t8 * t9 * t18 * 960.0F) + COM_Y *
+                    Jw * Mb * q1 * q2 * rk * t2 * t8 * t9 * t10 * t18 * 480.0F)
+                   + COM_X * Jw * Mb * q2 * q4 * rk * t3 * t8 * t9 * t10 * t18 *
+                   480.0F) + COM_Y * Jw * Mb * q3 * q4 * rk * t2 * t3 * t8 * t10
+                  * t18 * 480.0F) + COM_Z * Jw * Mb * rk * t2 * t8 * t9 * t11 *
+                 t18 * 288.0F) + COM_Z * Jw * Mb * rk * t7 * t8 * t9 * t10 * t18
+                * 288.0F) + COM_X * Jw * Mb * q1 * q3 * rk * t2 * t3 * t8 * t11 *
+               t18 * 864.0F) + COM_X * Jw * Mb * q1 * q3 * rk * t2 * t4 * t8 *
+              t10 * t18 * 960.0F) + COM_X * Jw * Mb * q1 * q3 * rk * t3 * t7 *
+             t8 * t10 * t18 * 864.0F) + COM_Y * Jw * Mb * q3 * q4 * rk * t3 * t8
+            * t9 * t10 * t18 * 480.0F) + COM_Z * Jw * Mb * rk * t2 * t3 * t4 *
+           t8 * t9 * t18 * 480.0F) + COM_Z * Jw * Mb * rk * t2 * t3 * t8 * t11 *
+          t18 * 288.0F) + COM_Z * Jw * Mb * rk * t2 * t4 * t8 * t12 * t18 *
+         720.0F) + COM_Z * Jw * Mb * rk * t2 * t3 * t8 * t9 * t12 * t18 * 480.0F;
+  f12 = COM_Z * Jw * Mb * rk * t4 * t7 * t8 * t9 * t18 * 288.0F;
+  f13 = COM_Z * Jw * Mb * rk * t3 * t7 * t8 * t10 * t18;
+  f14 = COM_Z * Jw * Mb * rk * t3 * t7 * t8 * t12 * t18;
+  f15 = COM_Z * Jw * Mb * rk * t2 * t8 * t9 * t10 * t12;
+  f16 = COM_Z * Jw * Mb * rk * t2 * t3 * t4 * t8 * t10;
+  f17 = COM_Z * Jw * Mb * rk * t4 * t8 * t9;
+  f18 = COM_Z * Jw * Mb * rk * t3 * t4 * t8 * t9;
+  f19 = COM_Z * Jw * Mb * rk * t3 * t8;
+  f20 = COM_Z * Jw * Mb * rk * t4 * t8;
+  f21 = COM_Z * Jw * Mb * rk * t3 * t8;
+  f22 = q1 * q2 * q3 * q4 * t8;
+  f23 = q1 * q2 * q3 * q4;
+  f24 = q1 * q2 * q3 * q4;
+  f25 = q1 * q2 * q3;
+  f26 = COM_X * Jw * Mb * q2;
+  f27 = COM_Y * Jw * Mb;
+  f28 = COM_X * Jw;
+  f29 = COM_Y * Jw;
+  f30 = COM_X * Jw;
+  f11 = ((((((((((((((((((((((((((((((((((((((((((((((((f11 + f12) + f13 *
+    288.0F) + f14 * 288.0F) + f15 * t18 * 192.0F) + f16 * t18 * 192.0F) + f17 *
+    t11 * t18 * 288.0F) + f18 * t10 * t18 * 480.0F) + f19 * t11 * t12 * t18 *
+    288.0F) + f20 * t10 * t12 * t18 * 720.0F) + f21 * t9 * t10 * t12 * t18 *
+    480.0F) + f22 * t11 * t19 * t20 * t23 * 128.0F) + f23 * t4 * t8 * t19 * t20 *
+    t23 * 128.0F) + f24 * t8 * t12 * t19 * t20 * t23 * 128.0F) + f25 * q4 * t7 *
+    t8 * t19 * t20 * t23 * 128.0F) + f26 * q4 * rk * t2 * t7 * t8 * t18 * 48.0F)
+    + f27 * q1 * q2 * rk * t3 * t4 * t8 * t18 * 48.0F) + f28 * Mb * q1 * q3 * rk
+    * t8 * t16 * t18 * 48.0F) + f29 * Mb * q1 * q2 * rk * t8 * t10 * t11 * t18 *
+    48.0F) + f30 * Mb * q2 * q4 * rk * t3 * t4 * t8 * t18 * 48.0F) + COM_Y * Jw *
+    Mb * q3 * q4 * rk * t2 * t7 * t8 * t18 * 48.0F) + COM_X * Jw * Mb * q1 * q3 *
+    rk * t8 * t15 * t18 * 48.0F) + COM_Y * Jw * Mb * q3 * q4 * rk * t8 * t9 *
+    t12 * t18 * 48.0F) + COM_X * Mb * Mk * q2 * q4 * rk * t2 * t8 * t20 * 64.0F)
+    + COM_Y * Mb * Mk * q1 * q2 * rk * t3 * t8 * t20 * 64.0F) + COM_Y * Mb * Mk *
+    q1 * q2 * rk * t8 * t10 * t20 * 64.0F) + COM_X * Mb * Mk * q2 * q4 * rk * t3
+    * t8 * t20 * 64.0F) + COM_Y * Mb * Mk * q3 * q4 * rk * t2 * t8 * t20 * 64.0F)
+    + COM_Y * Mb * Mk * q3 * q4 * rk * t8 * t9 * t20 * 64.0F) + t2 * t3 * t8 *
+    t9 * t10 * t19 * t20 * t21 * 256.0F) + t2 * t3 * t8 * t9 * t10 * t19 * t20 *
+    t23 * 256.0F) + COM_X * COM_Y * q1 * q4 * t2 * t8 * t9 * t10 * t19 * t20 *
+    256.0F) + COM_X * COM_Z * q1 * q3 * t2 * t3 * t8 * t9 * t19 * t20 * 128.0F)
+                        + COM_X * COM_Z * q2 * q4 * t2 * t8 * t9 * t10 * t19 *
+                        t20 * 128.0F) + COM_X * COM_Y * q2 * q3 * t3 * t8 * t9 *
+                       t10 * t19 * t20 * 256.0F) + COM_Y * COM_Z * q1 * q2 * t2 *
+                      t8 * t9 * t10 * t19 * t20 * 128.0F) + COM_Y * COM_Z * q3 *
+                     q4 * t2 * t3 * t8 * t10 * t19 * t20 * 128.0F) + q1 * q2 *
+                    q3 * q4 * t3 * t8 * t10 * t19 * t20 * t23 * 256.0F) + q1 *
+                   q2 * q3 * q4 * t8 * t9 * t10 * t19 * t20 * t23 * 256.0F) + q1
+                  * q2 * q3 * q4 * t3 * t8 * t9 * t19 * t20 * t23 * 256.0F) + q1
+                 * q2 * q3 * q4 * t2 * t8 * t10 * t19 * t20 * t23 * 256.0F) + q1
+                * q2 * q3 * q4 * t2 * t3 * t8 * t19 * t20 * t23 * 256.0F) + q1 *
+               q2 * q3 * q4 * t2 * t8 * t9 * t19 * t20 * t23 * 256.0F) + COM_X *
+              COM_Y * q1 * q4 * t8 * t9 * t12 * t19 * t20 * 128.0F) + COM_X *
+             COM_Z * q1 * q3 * t8 * t9 * t12 * t19 * t20 * 64.0F) + COM_X *
+            COM_Z * q2 * q4 * t2 * t7 * t8 * t19 * t20 * 64.0F) + COM_X * COM_Y *
+           q2 * q3 * t8 * t10 * t11 * t19 * t20 * 128.0F) + COM_Y * COM_Z * q1 *
+          q2 * t8 * t10 * t11 * t19 * t20 * 64.0F) + COM_Y * COM_Z * q3 * q4 *
+         t2 * t7 * t8 * t19 * t20 * 64.0F) + COM_X * Jw * Mb * q2 * q4 * rk * t2
+    * t8 * t11 * t18 * 144.0F;
+  f12 = COM_X * Jw * Mb * q2 * q4 * rk * t2 * t8 * t12 * t18 * 240.0F;
+  f13 = COM_X * Jw * Mb * q2 * q4 * rk * t7 * t8 * t10 * t18 * 144.0F;
+  f14 = COM_X * Jw * Mb * q2 * q4 * rk * t7 * t8 * t9 * t18;
+  f15 = COM_Y * Jw * Mb * q1 * q2 * rk * t4 * t8 * t9 * t18;
+  f16 = COM_Y * Jw * Mb * q1 * q2 * rk * t3 * t8 * t12;
+  f17 = COM_Y * Jw * Mb * q1 * q2 * rk * t2 * t4 * t8;
+  f18 = COM_Y * Jw * Mb * q1 * q2 * rk * t3 * t7;
+  f19 = COM_X * Jw * Mb * q1 * q3 * rk * t3 * t4 * t8;
+  f20 = COM_X * Jw * Mb * q1 * q3 * rk * t4;
+  f21 = COM_X * Jw * Mb * q1 * q3 * rk * t3 * t8;
+  f22 = COM_X * Jw * Mb * q1 * q3 * rk * t2;
+  f23 = COM_X * Jw * Mb * q1 * q3 * rk;
+  f24 = COM_X * Jw * Mb * q1 * q3 * rk;
+  f25 = COM_Y * Jw * Mb * q1 * q2;
+  f26 = COM_Y * Jw * Mb * q1;
+  f27 = COM_Y * Jw * Mb * q1;
+  f28 = COM_Y * Jw * Mb;
+  f29 = COM_X * Jw * Mb;
+  f30 = COM_X * Jw;
+  f31 = COM_X * Jw;
+  f11 = (((((((((((((((((((((((((((((((((((((((((((((((((f11 + f12) + f13) + f14
+    * 240.0F) + f15 * 144.0F) + f16 * t18 * 144.0F) + f17 * t18 * 240.0F) + f18 *
+    t8 * t18 * 240.0F) + f19 * t9 * t18 * 192.0F) + f20 * t8 * t12 * t18 *
+    288.0F) + f21 * t9 * t12 * t18 * 192.0F) + f22 * t8 * t9 * t12 * t18 *
+    288.0F) + f23 * t7 * t8 * t12 * t18 * 480.0F) + f24 * t2 * t7 * t8 * t9 *
+    t18 * 288.0F) + f25 * rk * t8 * t9 * t11 * t18 * 240.0F) + f26 * q2 * rk *
+    t8 * t10 * t12 * t18 * 240.0F) + f27 * q2 * rk * t2 * t8 * t11 * t18 *
+    144.0F) + f28 * q1 * q2 * rk * t7 * t8 * t10 * t18 * 144.0F) + f29 * q2 * q4
+    * rk * t3 * t8 * t11 * t18 * 240.0F) + f30 * Mb * q2 * q4 * rk * t4 * t8 *
+    t10 * t18 * 240.0F) + f31 * Mb * q2 * q4 * rk * t4 * t8 * t9 * t18 * 144.0F)
+    + COM_X * Jw * Mb * q2 * q4 * rk * t3 * t8 * t12 * t18 * 144.0F) + COM_Y *
+    Jw * Mb * q3 * q4 * rk * t2 * t8 * t11 * t18 * 144.0F) + COM_Y * Jw * Mb *
+    q3 * q4 * rk * t2 * t4 * t8 * t18 * 240.0F) + COM_Y * Jw * Mb * q3 * q4 * rk
+    * t7 * t8 * t10 * t18 * 144.0F) + COM_Y * Jw * Mb * q3 * q4 * rk * t3 * t7 *
+    t8 * t18 * 240.0F) + COM_X * Jw * Mb * q1 * q3 * rk * t3 * t8 * t10 * t11 *
+    t18 * 288.0F) + COM_X * Jw * Mb * q1 * q3 * rk * t4 * t8 * t11 * t18 *
+    480.0F) + COM_X * Jw * Mb * q1 * q3 * rk * t3 * t4 * t8 * t10 * t18 * 288.0F)
+    + COM_X * Jw * Mb * q1 * q3 * rk * t2 * t8 * t10 * t11 * t18 * 192.0F) +
+    COM_X * Jw * Mb * q1 * q3 * rk * t7 * t8 * t11 * t18 * 288.0F) + COM_X * Jw *
+    Mb * q1 * q3 * rk * t2 * t7 * t8 * t10 * t18 * 192.0F) + COM_Y * Jw * Mb *
+    q3 * q4 * rk * t8 * t9 * t11 * t18 * 240.0F) + COM_Y * Jw * Mb * q3 * q4 *
+    rk * t4 * t8 * t9 * t18 * 144.0F) + COM_Y * Jw * Mb * q3 * q4 * rk * t8 *
+                        t10 * t12 * t18 * 240.0F) + COM_Y * Jw * Mb * q3 * q4 *
+                       rk * t3 * t8 * t12 * t18 * 144.0F) + COM_X * COM_Z * q1 *
+                      q3 * t3 * t8 * t9 * t10 * t19 * t20 * 128.0F) + COM_X *
+                     COM_Z * q2 * q4 * t2 * t3 * t8 * t10 * t19 * t20 * 128.0F)
+                    + COM_Y * COM_Z * q3 * q4 * t2 * t8 * t9 * t10 * t19 * t20 *
+                    128.0F) + COM_Y * COM_Z * q1 * q2 * t2 * t3 * t8 * t10 * t19
+                   * t20 * 128.0F) + COM_X * Jw * Mb * q1 * q3 * rk * t2 * t3 *
+                  t8 * t9 * t10 * t18 * 1920.0F) + COM_Z * Jw * Mb * rk * t2 *
+                 t4 * t8 * t9 * t10 * t18 * 576.0F) + COM_Z * Jw * Mb * rk * t2 *
+                t3 * t8 * t10 * t12 * t18 * 576.0F) + COM_X * Jw * Mb * q2 * q4 *
+               rk * t2 * t4 * t8 * t18 * 240.0F) + COM_X * Jw * Mb * q2 * q4 *
+              rk * t3 * t7 * t8 * t18 * 240.0F) + COM_Y * Jw * Mb * q1 * q2 * rk
+             * t3 * t8 * t11 * t18 * 240.0F) + COM_Y * Jw * Mb * q1 * q2 * rk *
+            t4 * t8 * t10 * t18 * 240.0F) + COM_Y * Jw * Mb * q3 * q4 * rk * t2 *
+           t8 * t12 * t18 * 240.0F) + COM_Y * Jw * Mb * q3 * q4 * rk * t7 * t8 *
+          t9 * t18 * 240.0F) + COM_X * Jw * Mb * q1 * q3 * rk * t8 * t9 * t10 *
+         t11 * t18 * 288.0F) + COM_X * Jw * Mb * q1 * q3 * rk * t8 * t11 * t12 *
+    t18 * 480.0F;
+  f12 = COM_X * Jw * Mb * q1 * q3 * rk * t8 * t9 * t10 * t12 * t18 * 288.0F;
+  f13 = COM_X * COM_Y * q1 * q4 * t3 * t8 * t12 * t19 * t20;
+  f14 = COM_X * COM_Y * q2 * q3 * t2 * t8 * t11 * t19 * t20;
+  f15 = COM_X * COM_Z * q1 * q3 * t8 * t10 * t12 * t19;
+  f16 = COM_X * COM_Z * q2 * q4 * t3 * t7 * t8 * t19;
+  f17 = COM_Y * COM_Z * q1 * q2 * t3 * t8 * t11;
+  f18 = COM_Y * COM_Z * q3 * q4 * t7 * t8 * t9;
+  f19 = COM_X * Jw * Mb * q2 * q4 * rk * t2 * t3;
+  f20 = COM_X * Jw * Mb * q2 * q4 * rk * t2 * t3;
+  f21 = COM_Y * Jw * Mb * q1 * q2 * rk * t3;
+  f22 = COM_Y * Jw * Mb * q3 * q4 * rk * t2;
+  f23 = COM_Y * Jw * Mb * q3 * q4 * rk;
+  f24 = COM_Y * Jw * Mb * q1 * q2 * rk;
+  f25 = COM_X * Jw * Mb * q1 * q3;
+  f26 = COM_X * Jw * Mb * q1 * q3;
+  f27 = COM_X * Jw * Mb * q1;
+  f28 = COM_X * Jw * Mb * q1;
+  f29 = COM_X * Jw * Mb;
+  f30 = COM_X * Jw * Mb;
+  f11 = ((((((((((((((((((((((((((((((((((((((((((((((((f11 + f12) + f13 *
+    128.0F) + f14 * 128.0F) + f15 * t20 * 64.0F) + f16 * t20 * 64.0F) + f17 *
+    t19 * t20 * 64.0F) + f18 * t19 * t20 * 64.0F) + f19 * t8 * t10 * t18 *
+    480.0F) + f20 * t8 * t9 * t18 * 480.0F) + f21 * t8 * t9 * t10 * t18 * 480.0F)
+    + f22 * t8 * t9 * t10 * t18 * 480.0F) + f23 * t2 * t3 * t8 * t9 * t18 *
+    480.0F) + f24 * t2 * t3 * t8 * t10 * t18 * 480.0F) + f25 * rk * t3 * t8 * t9
+    * t11 * t18 * 960.0F) + f26 * rk * t4 * t8 * t9 * t10 * t18 * 864.0F) + f27 *
+    q3 * rk * t3 * t8 * t10 * t12 * t18 * 864.0F) + f28 * q3 * rk * t2 * t8 * t9
+    * t11 * t18 * 864.0F) + f29 * q1 * q3 * rk * t2 * t8 * t10 * t12 * t18 *
+    960.0F) + f30 * q1 * q3 * rk * t7 * t8 * t9 * t10 * t18 * 864.0F) - t2 * t5 *
+    t13 * t24 * 162.0F) - t2 * t5 * t14 * t24 * 18.0F) - t3 * t5 * t13 * t24 *
+    18.0F) - t2 * t5 * t15 * t24 * 90.0F) - t3 * t5 * t14 * t24 * 162.0F) - t2 *
+    t5 * t16 * t24 * 162.0F) - t3 * t5 * t15 * t24 * 162.0F) - t3 * t5 * t16 *
+    t24 * 90.0F) - t5 * t9 * t13 * t24 * 90.0F) - t5 * t9 * t14 * t24 * 162.0F)
+    - t5 * t10 * t13 * t24 * 162.0F) - t5 * t9 * t15 * t24 * 162.0F) - t5 * t10 *
+    t14 * t24 * 90.0F) - t5 * t9 * t16 * t24 * 18.0F) - t5 * t10 * t15 * t24 *
+                       18.0F) - t5 * t10 * t16 * t24 * 162.0F) - t2 * t4 * t5 *
+                     t7 * t24 * 396.0F) - t3 * t4 * t5 * t7 * t24 * 396.0F) - t2
+                   * t4 * t5 * t11 * t24 * 1188.0F) - t2 * t4 * t5 * t12 * t24 *
+                  972.0F) - t3 * t4 * t5 * t11 * t24 * 396.0F) - t3 * t4 * t5 *
+                t12 * t24 * 180.0F) - t2 * t5 * t7 * t11 * t24 * 180.0F) - t4 *
+              t5 * t7 * t9 * t24 * 1188.0F) - t2 * t5 * t7 * t12 * t24 * 396.0F)
+            - t3 * t5 * t7 * t11 * t24 * 972.0F) - t4 * t5 * t7 * t10 * t24 *
+           1188.0F) - t3 * t5 * t7 * t12 * t24 * 1188.0F) - t4 * t5 * t9 * t11 *
+         t24 * 1188.0F) - t2 * t5 * t11 * t12 * t24 * 1188.0F;
+  f12 = t4 * t5 * t9 * t12 * t24 * 180.0F;
+  f13 = t4 * t5 * t10 * t11 * t24 * 396.0F;
+  f14 = t3 * t5 * t11 * t12 * t24;
+  f15 = t4 * t5 * t10 * t12 * t24;
+  f16 = t5 * t7 * t9 * t11;
+  f17 = t5 * t7 * t9 * t12;
+  f18 = t5 * t7 * t10;
+  f19 = t5 * t7 * t10;
+  f20 = t5 * t9;
+  f21 = t5 * t10;
+  f22 = t2 * t3;
+  f23 = t2 * t3;
+  f11 = (((((((((((((((((((((((((((((((((((((((((((((((((f11 - f12) - f13) - f14
+    * 1188.0F) - f15 * 972.0F) - f16 * t24 * 972.0F) - f17 * t24 * 396.0F) - f18
+    * t11 * t24 * 180.0F) - f19 * t12 * t24 * 1188.0F) - f20 * t11 * t12 * t24 *
+    396.0F) - f21 * t11 * t12 * t24 * 396.0F) - f22 * t4 * t5 * t9 * t24 *
+    648.0F) - f23 * t4 * t5 * t10 * t24 * 792.0F) - t2 * t3 * t5 * t7 * t9 * t24
+    * 792.0F) - t2 * t3 * t5 * t7 * t10 * t24 * 648.0F) - t2 * t3 * t5 * t9 *
+    t11 * t24 * 2376.0F) - t2 * t4 * t5 * t9 * t10 * t24 * 2376.0F) - t2 * t3 *
+    t5 * t9 * t12 * t24 * 648.0F) - t2 * t3 * t5 * t10 * t11 * t24 * 648.0F) -
+    t3 * t4 * t5 * t9 * t10 * t24 * 648.0F) - t2 * t3 * t5 * t10 * t12 * t24 *
+    2376.0F) - t2 * t5 * t7 * t9 * t10 * t24 * 648.0F) - t3 * t5 * t7 * t9 * t10
+    * t24 * 2376.0F) - t2 * t5 * t9 * t10 * t11 * t24 * 648.0F) - t2 * t5 * t9 *
+    t10 * t12 * t24 * 792.0F) - t3 * t5 * t9 * t10 * t11 * t24 * 792.0F) - t3 *
+    t5 * t9 * t10 * t12 * t24 * 648.0F) - t4 * t7 * t8 * t19 * t20 * t22 * 32.0F)
+    - t4 * t8 * t11 * t19 * t20 * t22 * 32.0F) - t7 * t8 * t12 * t19 * t20 * t22
+    * 32.0F) - t8 * t11 * t12 * t19 * t20 * t22 * 32.0F) - COM_Z * Jk * Mb * rk *
+    t4 * t20 * 32.0F) - COM_Z * Jk * Mb * rk * t12 * t20 * 32.0F) - Jk * Jw * t2
+    * t4 * t8 * t18 * 120.0F) - Jk * Jw * t3 * t4 * t8 * t18 * 24.0F) - Jk * Jw *
+                       t2 * t7 * t8 * t18 * 24.0F) - Jk * Jw * t3 * t7 * t8 *
+                      t18 * 120.0F) - Jk * Jw * t2 * t8 * t11 * t18 * 72.0F) -
+                    Jk * Jw * t4 * t8 * t9 * t18 * 72.0F) - Jk * Jw * t2 * t8 *
+                   t12 * t18 * 120.0F) - Jk * Jw * t3 * t8 * t11 * t18 * 120.0F)
+                 - Jk * Jw * t4 * t8 * t10 * t18 * 120.0F) - Jk * Jw * t3 * t8 *
+                t12 * t18 * 72.0F) - Jk * Jw * t7 * t8 * t9 * t18 * 120.0F) - Jk
+              * Jw * t7 * t8 * t10 * t18 * 72.0F) - Jk * Jw * t8 * t9 * t11 *
+             t18 * 120.0F) - Jk * Jw * t8 * t9 * t12 * t18 * 24.0F) - Jk * Jw *
+           t8 * t10 * t11 * t18 * 24.0F) - Jk * Jw * t8 * t10 * t12 * t18 *
+          120.0F) - Jw * Mb * t2 * t4 * t18 * t24 * 120.0F) - Jw * Mb * t3 * t4 *
+    t18 * t24 * 24.0F;
+  f12 = Jw * Mb * t2 * t7 * t18 * t24 * 24.0F;
+  f13 = Jw * Mb * t3 * t7 * t18 * t24;
+  f14 = Jw * Mb * t2 * t11 * t18 * t24;
+  f15 = Jw * Mb * t4 * t9 * t18;
+  f16 = Jw * Mb * t2 * t12 * t18;
+  f17 = Jw * Mb * t3 * t11;
+  f18 = Jw * Mb * t4 * t10;
+  f19 = Jw * Mb * t3;
+  f20 = Jw * Mb * t7;
+  f21 = Jw * Mb;
+  f22 = Jw * Mb;
+  f11 = ((((((((((((((((((((((((((((((((((((((((((((((((f11 - f12) - f13 *
+    120.0F) - f14 * 72.0F) - f15 * t24 * 72.0F) - f16 * t24 * 120.0F) - f17 *
+    t18 * t24 * 120.0F) - f18 * t18 * t24 * 120.0F) - f19 * t12 * t18 * t24 *
+    72.0F) - f20 * t9 * t18 * t24 * 120.0F) - f21 * t7 * t10 * t18 * t24 * 72.0F)
+    - f22 * t9 * t11 * t18 * t24 * 120.0F) - Jw * Mb * t9 * t12 * t18 * t24 *
+    24.0F) - Jw * Mb * t10 * t11 * t18 * t24 * 24.0F) - Jw * Mb * t10 * t12 *
+    t18 * t24 * 120.0F) - Jw * Mk * t2 * t4 * t18 * t24 * 120.0F) - Jw * Mk * t3
+    * t4 * t18 * t24 * 24.0F) - Jw * Mk * t2 * t7 * t18 * t24 * 24.0F) - Jw * Mk
+    * t3 * t7 * t18 * t24 * 120.0F) - Jw * Mk * t2 * t11 * t18 * t24 * 72.0F) -
+    Jw * Mk * t4 * t9 * t18 * t24 * 72.0F) - Jw * Mk * t2 * t12 * t18 * t24 *
+    120.0F) - Jw * Mk * t3 * t11 * t18 * t24 * 120.0F) - Jw * Mk * t4 * t10 *
+    t18 * t24 * 120.0F) - Jw * Mk * t3 * t12 * t18 * t24 * 72.0F) - Jw * Mk * t7
+    * t9 * t18 * t24 * 120.0F) - Jw * Mk * t7 * t10 * t18 * t24 * 72.0F) - Jw *
+    Mk * t9 * t11 * t18 * t24 * 120.0F) - Jw * Mk * t9 * t12 * t18 * t24 * 24.0F)
+    - Jw * Mk * t10 * t11 * t18 * t24 * 24.0F) - Jw * Mk * t10 * t12 * t18 * t24
+    * 120.0F) - COM_Z * rk * t4 * t8 * t19 * t20 * 32.0F) - COM_Z * rk * t8 *
+    t12 * t19 * t20 * 32.0F) - COM_Z * Jk * Mb * rk * t3 * t9 * t20 * 64.0F) -
+                       COM_Z * Jw * Mb * rk * t8 * t13 * t18 * 24.0F) - COM_Z *
+                      Jw * Mb * rk * t8 * t16 * t18 * 24.0F) - COM_Z * Mb * Mk *
+                     rk * t4 * t8 * t20 * 32.0F) - COM_Z * Mb * Mk * rk * t8 *
+                    t12 * t20 * 32.0F) - Jk * Jw * t2 * t3 * t8 * t9 * t18 *
+                   240.0F) - Jk * Jw * t2 * t3 * t8 * t10 * t18 * 240.0F) - Jk *
+                 Jw * t2 * t8 * t9 * t10 * t18 * 240.0F) - Jk * Jw * t3 * t8 *
+                t9 * t10 * t18 * 240.0F) - Jw * Mb * t2 * t3 * t9 * t18 * t24 *
+               240.0F) - Jw * Mb * t2 * t3 * t10 * t18 * t24 * 240.0F) - Jw * Mb
+             * t2 * t9 * t10 * t18 * t24 * 240.0F) - Jw * Mb * t3 * t9 * t10 *
+            t18 * t24 * 240.0F) - Jw * Mk * t2 * t3 * t9 * t18 * t24 * 240.0F) -
+          Jw * Mk * t2 * t3 * t10 * t18 * t24 * 240.0F) - Jw * Mk * t2 * t9 *
+         t10 * t18 * t24 * 240.0F) - Jw * Mk * t3 * t9 * t10 * t18 * t24 *
+    240.0F;
+  f12 = COM_Z * rk * t3 * t8 * t9 * t19 * t20 * 64.0F;
+  f13 = t2 * t4 * t8 * t10 * t19 * t20 * t22 * 64.0F;
+  f14 = t3 * t7 * t8 * t9 * t19 * t20 * t22;
+  f15 = t3 * t8 * t9 * t11 * t19 * t20 * t22;
+  f16 = t2 * t8 * t10 * t12 * t19 * t20;
+  f17 = COM_X * Jk * Mb * q1 * q3 * rk * t2;
+  f18 = COM_X * Jk * Mb * q1 * q3 * rk;
+  f19 = COM_X * Jk * Mb * q1 * q3 * rk;
+  f20 = COM_X * Jk * Mb * q1 * q3;
+  f21 = COM_Z * Jw * Mb * rk * t2;
+  f22 = COM_Z * Jw * Mb * rk;
+  f23 = COM_Z * Jw * Mb * rk;
+  f24 = COM_Z * Jw * Mb;
+  f25 = COM_Z * Jw * Mb;
+  f26 = COM_Z * Jw;
+  f27 = COM_Z * Jw;
+  f11 = (((((((((((((((((((((((((((((((((((((((((((((((((f11 - f12) - f13) - f14
+    * 64.0F) - f15 * 64.0F) - f16 * t22 * 64.0F) - f17 * t20 * 64.0F) - f18 * t3
+    * t20 * 64.0F) - f19 * t9 * t20 * 64.0F) - f20 * rk * t10 * t20 * 64.0F) -
+    f21 * t8 * t14 * t18 * 24.0F) - f22 * t4 * t8 * t12 * t18 * 144.0F) - f23 *
+    t2 * t8 * t15 * t18 * 120.0F) - f24 * rk * t3 * t8 * t14 * t18 * 120.0F) -
+    f25 * rk * t3 * t8 * t15 * t18 * 120.0F) - f26 * Mb * rk * t8 * t9 * t14 *
+    t18 * 120.0F) - f27 * Mb * rk * t8 * t9 * t15 * t18 * 120.0F) - COM_Z * Jw *
+    Mb * rk * t8 * t10 * t14 * t18 * 120.0F) - COM_Z * Jw * Mb * rk * t8 * t10 *
+    t15 * t18 * 24.0F) - COM_Z * Mb * Mk * rk * t3 * t8 * t9 * t20 * 64.0F) -
+    COM_X * q1 * q3 * rk * t2 * t8 * t19 * t20 * 64.0F) - COM_X * q1 * q3 * rk *
+    t3 * t8 * t19 * t20 * 64.0F) - COM_X * q1 * q3 * rk * t8 * t9 * t19 * t20 *
+    64.0F) - COM_X * q1 * q3 * rk * t8 * t10 * t19 * t20 * 64.0F) - t2 * t3 * t8
+    * t9 * t10 * t19 * t20 * t22 * 128.0F) - COM_X * COM_Y * q1 * q4 * t2 * t4 *
+    t8 * t19 * t20 * 256.0F) - COM_X * COM_Y * q2 * q3 * t2 * t4 * t8 * t19 *
+    t20 * 128.0F) - COM_Y * COM_Z * q1 * q2 * t2 * t4 * t8 * t19 * t20 * 64.0F)
+    - COM_X * COM_Y * q1 * q4 * t3 * t4 * t8 * t19 * t20 * 128.0F) - COM_Y *
+    COM_Z * q1 * q2 * t3 * t4 * t8 * t19 * t20 * 64.0F) - COM_X * COM_Z * q2 *
+    q4 * t2 * t4 * t8 * t19 * t20 * 64.0F) - COM_X * COM_Y * q2 * q3 * t2 * t7 *
+    t8 * t19 * t20 * 128.0F) - COM_X * COM_Z * q1 * q3 * t2 * t7 * t8 * t19 *
+    t20 * 64.0F) - COM_X * COM_Z * q2 * q4 * t3 * t4 * t8 * t19 * t20 * 64.0F) -
+                        COM_X * COM_Y * q1 * q4 * t3 * t7 * t8 * t19 * t20 *
+                        128.0F) - COM_X * COM_Y * q2 * q3 * t3 * t7 * t8 * t19 *
+                       t20 * 256.0F) - COM_X * COM_Z * q1 * q3 * t3 * t7 * t8 *
+                      t19 * t20 * 64.0F) - COM_Y * COM_Z * q3 * q4 * t2 * t4 *
+                     t8 * t19 * t20 * 64.0F) - COM_Y * COM_Z * q3 * q4 * t3 * t4
+                    * t8 * t19 * t20 * 64.0F) - COM_X * COM_Y * q1 * q4 * t4 *
+                   t8 * t9 * t19 * t20 * 128.0F) - COM_X * COM_Z * q1 * q3 * t2 *
+                  t8 * t11 * t19 * t20 * 192.0F) - COM_Y * COM_Z * q1 * q2 * t4 *
+                 t8 * t9 * t19 * t20 * 192.0F) - COM_X * COM_Y * q1 * q4 * t3 *
+                t8 * t11 * t19 * t20 * 128.0F) - COM_X * COM_Y * q1 * q4 * t4 *
+               t8 * t10 * t19 * t20 * 256.0F) - COM_X * COM_Y * q2 * q3 * t2 *
+              t8 * t12 * t19 * t20 * 128.0F) - COM_X * COM_Z * q1 * q3 * t3 * t8
+             * t11 * t19 * t20 * 64.0F) - COM_Y * COM_Z * q1 * q2 * t2 * t8 *
+            t12 * t19 * t20 * 64.0F) - COM_Y * COM_Z * q1 * q2 * t4 * t8 * t10 *
+           t19 * t20 * 64.0F) - COM_X * COM_Z * q2 * q4 * t4 * t8 * t9 * t19 *
+          t20 * 192.0F) - COM_Y * COM_Z * q1 * q2 * t3 * t8 * t12 * t19 * t20 *
+         192.0F) - COM_X * COM_Y * q2 * q3 * t7 * t8 * t9 * t19 * t20 * 256.0F;
+  f12 = COM_X * COM_Z * q1 * q3 * t7 * t8 * t9 * t19 * t20 * 64.0F;
+  f13 = COM_X * COM_Z * q2 * q4 * t2 * t8 * t12 * t19 * t20;
+  f14 = COM_X * COM_Z * q2 * q4 * t4 * t8 * t10 * t19 * t20;
+  f15 = COM_X * COM_Y * q2 * q3 * t7 * t8 * t10 * t19;
+  f16 = COM_X * COM_Z * q1 * q3 * t7 * t8 * t10 * t19;
+  f17 = COM_X * COM_Z * q2 * q4 * t3 * t8 * t12;
+  f18 = COM_Y * COM_Z * q3 * q4 * t4 * t8 * t9;
+  f19 = COM_Y * COM_Z * q3 * q4 * t2 * t8;
+  f20 = COM_Y * COM_Z * q3 * q4 * t4 * t8;
+  f21 = COM_Y * COM_Z * q3 * q4 * t3;
+  f22 = COM_X * COM_Z * q1 * q3 * t8;
+  f23 = COM_X * COM_Z * q1 * q3;
+  f24 = COM_Y * COM_Z * q1 * q2;
+  f25 = COM_Y * COM_Z * q1;
+  f26 = COM_X * COM_Z * q2;
+  f27 = COM_X * COM_Z;
+  f28 = COM_Y * COM_Z;
+  f11 = ((((((((((((((((((((((((((((((((((((((((((((((((f11 - f12) - f13 * 64.0F)
+    - f14 * 64.0F) - f15 * t20 * 128.0F) - f16 * t20 * 192.0F) - f17 * t19 * t20
+    * 192.0F) - f18 * t19 * t20 * 192.0F) - f19 * t12 * t19 * t20 * 64.0F) - f20
+    * t10 * t19 * t20 * 64.0F) - f21 * t8 * t12 * t19 * t20 * 192.0F) - f22 * t9
+    * t11 * t19 * t20 * 64.0F) - f23 * t8 * t10 * t11 * t19 * t20 * 64.0F) - f24
+    * t8 * t9 * t12 * t19 * t20 * 64.0F) - f25 * q2 * t8 * t10 * t12 * t19 * t20
+    * 64.0F) - f26 * q4 * t8 * t9 * t12 * t19 * t20 * 64.0F) - f27 * q2 * q4 *
+    t8 * t10 * t12 * t19 * t20 * 64.0F) - f28 * q3 * q4 * t8 * t9 * t12 * t19 *
+    t20 * 64.0F) - COM_Y * COM_Z * q3 * q4 * t8 * t10 * t12 * t19 * t20 * 64.0F)
+    - q1 * q2 * q3 * q4 * t4 * t8 * t19 * t20 * t21 * 128.0F) - q1 * q2 * q3 *
+    q4 * t7 * t8 * t19 * t20 * t21 * 128.0F) - q1 * q2 * q3 * q4 * t8 * t11 *
+    t19 * t20 * t21 * 128.0F) - q1 * q2 * q3 * q4 * t8 * t12 * t19 * t20 * t21 *
+    128.0F) - COM_Y * Jw * Mb * q1 * q2 * rk * t8 * t13 * t18 * 48.0F) - COM_Y *
+    Jw * Mb * q1 * q2 * rk * t8 * t14 * t18 * 48.0F) - COM_X * Jw * Mb * q2 * q4
+    * rk * t8 * t13 * t18 * 48.0F) - COM_Y * Jw * Mb * q1 * q2 * rk * t8 * t15 *
+    t18 * 48.0F) - COM_X * Jw * Mb * q2 * q4 * rk * t8 * t14 * t18 * 48.0F) -
+    COM_Y * Jw * Mb * q1 * q2 * rk * t8 * t16 * t18 * 48.0F) - COM_X * Jw * Mb *
+    q2 * q4 * rk * t8 * t15 * t18 * 48.0F) - COM_Y * Jw * Mb * q3 * q4 * rk * t8
+    * t13 * t18 * 48.0F) - COM_X * Jw * Mb * q2 * q4 * rk * t8 * t16 * t18 *
+    48.0F) - COM_Y * Jw * Mb * q3 * q4 * rk * t8 * t14 * t18 * 48.0F) - COM_Y *
+                        Jw * Mb * q3 * q4 * rk * t8 * t15 * t18 * 48.0F) - COM_Y
+                       * Jw * Mb * q3 * q4 * rk * t8 * t16 * t18 * 48.0F) -
+                      COM_X * Mb * Mk * q1 * q3 * rk * t2 * t8 * t20 * 64.0F) -
+                     COM_X * Mb * Mk * q1 * q3 * rk * t3 * t8 * t20 * 64.0F) -
+                    COM_X * Mb * Mk * q1 * q3 * rk * t8 * t9 * t20 * 64.0F) -
+                   COM_X * Mb * Mk * q1 * q3 * rk * t8 * t10 * t20 * 64.0F) -
+                  COM_Z * Jw * Mb * rk * t2 * t3 * t4 * t8 * t18 * 96.0F) -
+                 COM_Z * Jw * Mb * rk * t2 * t4 * t7 * t8 * t18 * 96.0F) - COM_Z
+                * Jw * Mb * rk * t2 * t4 * t8 * t9 * t18 * 288.0F) - COM_Z * Jw *
+               Mb * rk * t3 * t4 * t8 * t9 * t18 * 96.0F) - COM_Z * Jw * Mb * rk
+              * t2 * t3 * t8 * t12 * t18 * 288.0F) - COM_Z * Jw * Mb * rk * t2 *
+             t4 * t8 * t11 * t18 * 288.0F) - COM_Z * Jw * Mb * rk * t3 * t4 * t8
+            * t10 * t18 * 96.0F) - COM_Z * Jw * Mb * rk * t2 * t7 * t8 * t11 *
+           t18 * 240.0F) - COM_Z * Jw * Mb * rk * t2 * t7 * t8 * t12 * t18 *
+          96.0F) - COM_Z * Jw * Mb * rk * t3 * t7 * t8 * t11 * t18 * 720.0F) -
+    COM_Z * Jw * Mb * rk * t4 * t7 * t8 * t10 * t18 * 288.0F;
+  f12 = COM_Z * Jw * Mb * rk * t2 * t8 * t9 * t12 * t18 * 96.0F;
+  f13 = COM_Z * Jw * Mb * rk * t4 * t8 * t9 * t10 * t18 * 288.0F;
+  f14 = COM_Z * Jw * Mb * rk * t3 * t8 * t9 * t12 * t18;
+  f15 = COM_Z * Jw * Mb * rk * t2 * t8 * t11 * t12 * t18;
+  f16 = COM_Z * Jw * Mb * rk * t3 * t8 * t10 * t12;
+  f17 = COM_Z * Jw * Mb * rk * t4 * t8 * t10 * t11;
+  f18 = COM_Z * Jw * Mb * rk * t7 * t8 * t9;
+  f19 = COM_Z * Jw * Mb * rk * t7 * t8 * t10;
+  f20 = COM_Z * Jw * Mb * rk * t7 * t8;
+  f21 = COM_Z * Jw * Mb * rk * t8 * t9;
+  f22 = COM_Z * Jw * Mb * rk * t8;
+  f23 = COM_X * Jw * Mb * q1 * q3 * rk;
+  f24 = COM_X * Jw * Mb * q1 * q3;
+  f25 = COM_X * Jw * Mb * q1 * q3;
+  f26 = COM_X * Jw * Mb * q1;
+  f27 = COM_Y * Jw * Mb * q1;
+  f28 = COM_X * Jw * Mb;
+  f29 = COM_X * Jw * Mb;
+  f30 = COM_X * Jw;
+  f31 = COM_X * Jw;
+  f11 = (((((((((((((((((((((((((((((((((((((((((((((((((f11 - f12) - f13) - f14
+    * 96.0F) - f15 * 288.0F) - f16 * t18 * 288.0F) - f17 * t18 * 96.0F) - f18 *
+    t11 * t18 * 720.0F) - f19 * t11 * t18 * 240.0F) - f20 * t10 * t12 * t18 *
+    288.0F) - f21 * t10 * t12 * t18 * 96.0F) - f22 * t10 * t11 * t12 * t18 *
+    96.0F) - f23 * t2 * t4 * t8 * t18 * 240.0F) - f24 * rk * t3 * t4 * t8 * t18 *
+    48.0F) - f25 * rk * t2 * t7 * t8 * t18 * 48.0F) - f26 * q3 * rk * t3 * t7 *
+    t8 * t18 * 240.0F) - f27 * q2 * rk * t4 * t7 * t8 * t18 * 480.0F) - f28 * q1
+    * q3 * rk * t2 * t8 * t11 * t18 * 144.0F) - f29 * q1 * q3 * rk * t4 * t8 *
+    t9 * t18 * 144.0F) - f30 * Mb * q2 * q4 * rk * t4 * t7 * t8 * t18 * 480.0F)
+    - f31 * Mb * q1 * q3 * rk * t2 * t8 * t12 * t18 * 240.0F) - COM_X * Jw * Mb *
+    q1 * q3 * rk * t3 * t8 * t11 * t18 * 240.0F) - COM_X * Jw * Mb * q1 * q3 *
+    rk * t4 * t8 * t10 * t18 * 240.0F) - COM_X * Jw * Mb * q1 * q3 * rk * t3 *
+    t8 * t12 * t18 * 144.0F) - COM_Y * Jw * Mb * q1 * q2 * rk * t4 * t8 * t11 *
+    t18 * 480.0F) - COM_Y * Jw * Mb * q3 * q4 * rk * t4 * t7 * t8 * t18 * 480.0F)
+    - COM_X * Jw * Mb * q1 * q3 * rk * t7 * t8 * t9 * t18 * 240.0F) - COM_Y * Jw
+    * Mb * q1 * q2 * rk * t4 * t8 * t12 * t18 * 288.0F) - COM_X * Jw * Mb * q1 *
+    q3 * rk * t7 * t8 * t10 * t18 * 144.0F) - COM_X * Jw * Mb * q2 * q4 * rk *
+    t4 * t8 * t11 * t18 * 480.0F) - COM_X * Jw * Mb * q2 * q4 * rk * t4 * t8 *
+    t12 * t18 * 288.0F) - COM_Y * Jw * Mb * q1 * q2 * rk * t7 * t8 * t11 * t18 *
+    288.0F) - COM_Y * Jw * Mb * q1 * q2 * rk * t7 * t8 * t12 * t18 * 480.0F) -
+    COM_Y * Jw * Mb * q3 * q4 * rk * t4 * t8 * t11 * t18 * 480.0F) - COM_X * Jw *
+                        Mb * q1 * q3 * rk * t8 * t9 * t11 * t18 * 240.0F) -
+                       COM_X * Jw * Mb * q2 * q4 * rk * t7 * t8 * t11 * t18 *
+                       288.0F) - COM_Y * Jw * Mb * q3 * q4 * rk * t4 * t8 * t12 *
+                      t18 * 288.0F) - COM_X * Jw * Mb * q1 * q3 * rk * t8 * t9 *
+                     t12 * t18 * 48.0F) - COM_X * Jw * Mb * q1 * q3 * rk * t8 *
+                    t10 * t11 * t18 * 48.0F) - COM_X * Jw * Mb * q2 * q4 * rk *
+                   t7 * t8 * t12 * t18 * 480.0F) - COM_X * Jw * Mb * q1 * q3 *
+                  rk * t8 * t10 * t12 * t18 * 240.0F) - COM_Y * Jw * Mb * q3 *
+                 q4 * rk * t7 * t8 * t11 * t18 * 288.0F) - COM_Y * Jw * Mb * q1 *
+                q2 * rk * t8 * t11 * t12 * t18 * 480.0F) - COM_Y * Jw * Mb * q3 *
+               q4 * rk * t7 * t8 * t12 * t18 * 480.0F) - COM_X * Jw * Mb * q2 *
+              q4 * rk * t8 * t11 * t12 * t18 * 480.0F) - COM_Y * Jw * Mb * q3 *
+             q4 * rk * t8 * t11 * t12 * t18 * 480.0F) - COM_Z * Jw * Mb * rk *
+            t2 * t3 * t7 * t8 * t9 * t18 * 192.0F) - COM_Z * Jw * Mb * rk * t2 *
+           t3 * t7 * t8 * t10 * t18 * 480.0F) - COM_Z * Jw * Mb * rk * t2 * t3 *
+          t8 * t9 * t11 * t18 * 576.0F) - COM_Z * Jw * Mb * rk * t2 * t3 * t8 *
+         t10 * t11 * t18 * 480.0F) - COM_Z * Jw * Mb * rk * t2 * t7 * t8 * t9 *
+    t10 * t18 * 480.0F;
+  f12 = COM_Z * Jw * Mb * rk * t3 * t7 * t8 * t9 * t10 * t18 * 576.0F;
+  f13 = COM_Z * Jw * Mb * rk * t2 * t8 * t9 * t10 * t11 * t18;
+  f14 = COM_Z * Jw * Mb * rk * t3 * t8 * t9 * t10 * t11 * t18;
+  f15 = COM_X * COM_Y * q2 * q3 * t2 * t3 * t8 * t9 * t19;
+  f16 = COM_Y * COM_Z * q1 * q2 * t2 * t3 * t8 * t9 * t19;
+  f17 = COM_X * COM_Y * q1 * q4 * t2 * t3 * t8 * t10;
+  f18 = COM_X * COM_Z * q1 * q3 * t2 * t3 * t8 * t10;
+  f19 = COM_X * COM_Z * q2 * q4 * t2 * t3 * t8;
+  f20 = COM_Y * COM_Z * q3 * q4 * t2 * t3 * t8;
+  f21 = COM_X * COM_Z * q1 * q3 * t2 * t8;
+  f22 = COM_Y * COM_Z * q1 * q2 * t3 * t8;
+  f23 = COM_X * COM_Z * q2 * q4 * t3;
+  f24 = COM_Y * COM_Z * q3 * q4 * t3;
+  f25 = q1 * q2 * q3 * q4;
+  f26 = q1 * q2 * q3 * q4;
+  f27 = q1 * q2 * q3;
+  f28 = q1 * q2 * q3;
+  f29 = q1 * q2;
+  f30 = q1 * q2;
+  f31 = COM_Y * Jw;
+  t24 = COM_X * Jw;
+  f11 = ((((((((((((((((((((((((((((((((((((((((((((((((f11 - f12) - f13 *
+    480.0F) - f14 * 192.0F) - f15 * t20 * 256.0F) - f16 * t20 * 128.0F) - f17 *
+    t19 * t20 * 256.0F) - f18 * t19 * t20 * 128.0F) - f19 * t9 * t19 * t20 *
+    128.0F) - f20 * t9 * t19 * t20 * 128.0F) - f21 * t9 * t10 * t19 * t20 *
+    128.0F) - f22 * t9 * t10 * t19 * t20 * 128.0F) - f23 * t8 * t9 * t10 * t19 *
+    t20 * 128.0F) - f24 * t8 * t9 * t10 * t19 * t20 * 128.0F) - f25 * t2 * t3 *
+    t8 * t19 * t20 * t21 * 256.0F) - f26 * t2 * t8 * t9 * t19 * t20 * t21 *
+    256.0F) - f27 * q4 * t2 * t8 * t10 * t19 * t20 * t21 * 256.0F) - f28 * q4 *
+    t3 * t8 * t9 * t19 * t20 * t21 * 256.0F) - f29 * q3 * q4 * t3 * t8 * t10 *
+    t19 * t20 * t21 * 256.0F) - f30 * q3 * q4 * t8 * t9 * t10 * t19 * t20 * t21 *
+    256.0F) - f31 * Mb * q1 * q2 * rk * t2 * t3 * t4 * t8 * t18 * 288.0F) - t24 *
+    Mb * q2 * q4 * rk * t2 * t3 * t4 * t8 * t18 * 288.0F) - COM_Y * Jw * Mb * q1
+    * q2 * rk * t2 * t3 * t7 * t8 * t18 * 288.0F) - COM_Y * Jw * Mb * q3 * q4 *
+    rk * t2 * t3 * t4 * t8 * t18 * 288.0F) - COM_X * Jw * Mb * q1 * q3 * rk * t2
+    * t3 * t8 * t9 * t18 * 480.0F) - COM_X * Jw * Mb * q2 * q4 * rk * t2 * t3 *
+    t7 * t8 * t18 * 288.0F) - COM_X * Jw * Mb * q1 * q3 * rk * t2 * t3 * t8 *
+    t10 * t18 * 480.0F) - COM_Y * Jw * Mb * q1 * q2 * rk * t2 * t4 * t8 * t9 *
+    t18 * 864.0F) - COM_Y * Jw * Mb * q1 * q2 * rk * t2 * t3 * t8 * t11 * t18 *
+    864.0F) - COM_Y * Jw * Mb * q1 * q2 * rk * t2 * t4 * t8 * t10 * t18 * 960.0F)
+    - COM_Y * Jw * Mb * q1 * q2 * rk * t3 * t4 * t8 * t9 * t18 * 192.0F) - COM_Y
+    * Jw * Mb * q3 * q4 * rk * t2 * t3 * t7 * t8 * t18 * 288.0F) - COM_X * Jw *
+    Mb * q2 * q4 * rk * t2 * t4 * t8 * t9 * t18 * 864.0F) - COM_Y * Jw * Mb * q1
+                        * q2 * rk * t2 * t3 * t8 * t12 * t18 * 864.0F) - COM_Y *
+                       Jw * Mb * q1 * q2 * rk * t3 * t4 * t8 * t10 * t18 *
+                       288.0F) - COM_X * Jw * Mb * q2 * q4 * rk * t2 * t3 * t8 *
+                      t11 * t18 * 864.0F) - COM_X * Jw * Mb * q2 * q4 * rk * t2 *
+                     t4 * t8 * t10 * t18 * 960.0F) - COM_X * Jw * Mb * q2 * q4 *
+                    rk * t3 * t4 * t8 * t9 * t18 * 192.0F) - COM_Y * Jw * Mb *
+                   q1 * q2 * rk * t2 * t7 * t8 * t9 * t18 * 288.0F) - COM_X * Jw
+                  * Mb * q2 * q4 * rk * t2 * t3 * t8 * t12 * t18 * 864.0F) -
+                 COM_X * Jw * Mb * q2 * q4 * rk * t3 * t4 * t8 * t10 * t18 *
+                 288.0F) - COM_Y * Jw * Mb * q1 * q2 * rk * t2 * t7 * t8 * t10 *
+                t18 * 192.0F) - COM_Y * Jw * Mb * q1 * q2 * rk * t3 * t7 * t8 *
+               t9 * t18 * 960.0F) - COM_Y * Jw * Mb * q3 * q4 * rk * t2 * t4 *
+              t8 * t9 * t18 * 864.0F) - COM_X * Jw * Mb * q2 * q4 * rk * t2 * t7
+             * t8 * t9 * t18 * 288.0F) - COM_Y * Jw * Mb * q1 * q2 * rk * t3 *
+            t7 * t8 * t10 * t18 * 864.0F) - COM_Y * Jw * Mb * q3 * q4 * rk * t2 *
+           t3 * t8 * t11 * t18 * 864.0F) - COM_Y * Jw * Mb * q3 * q4 * rk * t2 *
+          t4 * t8 * t10 * t18 * 960.0F) - COM_Y * Jw * Mb * q3 * q4 * rk * t3 *
+         t4 * t8 * t9 * t18 * 192.0F) - COM_X * Jw * Mb * q1 * q3 * rk * t2 * t8
+    * t9 * t10 * t18 * 480.0F;
+  f12 = COM_X * Jw * Mb * q2 * q4 * rk * t2 * t7 * t8 * t10 * t18 * 192.0F;
+  f13 = COM_X * Jw * Mb * q2 * q4 * rk * t3 * t7 * t8 * t9 * t18 * 960.0F;
+  f14 = COM_Y * Jw * Mb * q3 * q4 * rk * t2 * t3 * t8 * t12 * t18;
+  f15 = COM_Y * Jw * Mb * q3 * q4 * rk * t3 * t4 * t8 * t10 * t18;
+  f16 = COM_X * Jw * Mb * q1 * q3 * rk * t3 * t8 * t9 * t10;
+  f17 = COM_X * Jw * Mb * q2 * q4 * rk * t3 * t7 * t8 * t10;
+  f18 = COM_Y * Jw * Mb * q1 * q2 * rk * t2 * t8 * t9;
+  f19 = COM_Y * Jw * Mb * q3 * q4 * rk * t2 * t7 * t8;
+  f20 = COM_Y * Jw * Mb * q1 * q2 * rk * t2 * t8;
+  f21 = COM_Y * Jw * Mb * q1 * q2 * rk * t2 * t8;
+  f22 = COM_Y * Jw * Mb * q1 * q2 * rk * t3;
+  f23 = COM_Y * Jw * Mb * q1 * q2 * rk * t4;
+  f24 = COM_Y * Jw * Mb * q3 * q4 * rk;
+  f25 = COM_Y * Jw * Mb * q3 * q4 * rk;
+  f26 = COM_X * Jw * Mb * q2 * q4;
+  f27 = COM_Y * Jw * Mb * q1 * q2;
+  f28 = COM_Y * Jw * Mb * q1;
+  f29 = COM_Y * Jw * Mb * q1;
+  f30 = COM_Y * Jw * Mb;
+  f31 = COM_X * Jw * Mb;
+  t24 = COM_X * Jw;
+  f32 = COM_X * Jw;
+  t476 = 1.0F / (((((((((((((((((((((((((((((((((((((((((((((((((f11 - f12) -
+    f13) - f14 * 864.0F) - f15 * 288.0F) - f16 * t18 * 480.0F) - f17 * t18 *
+    864.0F) - f18 * t11 * t18 * 864.0F) - f19 * t9 * t18 * 288.0F) - f20 * t9 *
+    t12 * t18 * 288.0F) - f21 * t10 * t11 * t18 * 192.0F) - f22 * t8 * t9 * t11 *
+    t18 * 960.0F) - f23 * t8 * t9 * t10 * t18 * 864.0F) - f24 * t2 * t7 * t8 *
+    t10 * t18 * 192.0F) - f25 * t3 * t7 * t8 * t9 * t18 * 960.0F) - f26 * rk *
+    t2 * t8 * t9 * t11 * t18 * 864.0F) - f27 * rk * t2 * t8 * t10 * t12 * t18 *
+    960.0F) - f28 * q2 * rk * t3 * t8 * t9 * t12 * t18 * 192.0F) - f29 * q2 * rk
+    * t3 * t8 * t10 * t11 * t18 * 288.0F) - f30 * q3 * q4 * rk * t3 * t7 * t8 *
+    t10 * t18 * 864.0F) - f31 * q2 * q4 * rk * t2 * t8 * t9 * t12 * t18 * 288.0F)
+    - t24 * Mb * q2 * q4 * rk * t2 * t8 * t10 * t11 * t18 * 192.0F) - f32 * Mb *
+    q2 * q4 * rk * t3 * t8 * t9 * t11 * t18 * 960.0F) - COM_X * Jw * Mb * q2 *
+    q4 * rk * t4 * t8 * t9 * t10 * t18 * 864.0F) - COM_Y * Jw * Mb * q1 * q2 *
+    rk * t3 * t8 * t10 * t12 * t18 * 864.0F) - COM_X * Jw * Mb * q2 * q4 * rk *
+    t2 * t8 * t10 * t12 * t18 * 960.0F) - COM_X * Jw * Mb * q2 * q4 * rk * t3 *
+    t8 * t9 * t12 * t18 * 192.0F) - COM_X * Jw * Mb * q2 * q4 * rk * t3 * t8 *
+    t10 * t11 * t18 * 288.0F) - COM_Y * Jw * Mb * q1 * q2 * rk * t7 * t8 * t9 *
+    t10 * t18 * 864.0F) - COM_Y * Jw * Mb * q3 * q4 * rk * t2 * t8 * t9 * t11 *
+    t18 * 864.0F) - COM_X * Jw * Mb * q2 * q4 * rk * t3 * t8 * t10 * t12 * t18 *
+    864.0F) - COM_Y * Jw * Mb * q3 * q4 * rk * t2 * t8 * t9 * t12 * t18 * 288.0F)
+    - COM_Y * Jw * Mb * q3 * q4 * rk * t2 * t8 * t10 * t11 * t18 * 192.0F) -
+    COM_Y * Jw * Mb * q3 * q4 * rk * t3 * t8 * t9 * t11 * t18 * 960.0F) - COM_Y *
+    Jw * Mb * q3 * q4 * rk * t4 * t8 * t9 * t10 * t18 * 864.0F) - COM_X * Jw *
+    Mb * q2 * q4 * rk * t7 * t8 * t9 * t10 * t18 * 864.0F) - COM_Y * Jw * Mb *
+    q3 * q4 * rk * t2 * t8 * t10 * t12 * t18 * 960.0F) - COM_Y * Jw * Mb * q3 *
+    q4 * rk * t3 * t8 * t9 * t12 * t18 * 192.0F) - COM_Y * Jw * Mb * q3 * q4 *
+    rk * t3 * t8 * t10 * t11 * t18 * 288.0F) - COM_Y * Jw * Mb * q3 * q4 * rk *
+    t3 * t8 * t10 * t12 * t18 * 864.0F) - COM_Y * Jw * Mb * q1 * q2 * rk * t8 *
+    t9 * t10 * t11 * t18 * 288.0F) - COM_Y * Jw * Mb * q3 * q4 * rk * t7 * t8 *
+    t9 * t10 * t18 * 864.0F) - COM_Y * Jw * Mb * q1 * q2 * rk * t8 * t9 * t10 *
+                        t12 * t18 * 288.0F) - COM_X * Jw * Mb * q2 * q4 * rk *
+                       t8 * t9 * t10 * t11 * t18 * 288.0F) - COM_X * Jw * Mb *
+                      q2 * q4 * rk * t8 * t9 * t10 * t12 * t18 * 288.0F) - COM_Y
+                     * Jw * Mb * q3 * q4 * rk * t8 * t9 * t10 * t11 * t18 *
+                     288.0F) - COM_Y * Jw * Mb * q3 * q4 * rk * t8 * t9 * t10 *
+                    t12 * t18 * 288.0F) - COM_Y * Jw * Mb * q1 * q2 * rk * t2 *
+                   t3 * t8 * t9 * t10 * t18 * 1920.0F) - COM_X * Jw * Mb * q2 *
+                  q4 * rk * t2 * t3 * t8 * t9 * t10 * t18 * 1920.0F) - COM_Y *
+                 Jw * Mb * q3 * q4 * rk * t2 * t3 * t8 * t9 * t10 * t18 *
+                 1920.0F);
+
+  // 'SteadyStateAcceleration:483' acceleration = [rk.*t476.*(q1.*q3.*t5.*t6.*t8.*t13.*-9.0+q1.*q3.*t5.*t6.*t8.*t14.*9.0+q1.*q3.*t5.*t6.*t8.*t15.*9.0-q2.*q4.*t5.*t6.*t8.*t13.*9.0-q1.*q3.*t5.*t6.*t8.*t16.*9.0+q2.*q4.*t5.*t6.*t8.*t14.*9.0+q2.*q4.*t5.*t6.*t8.*t15.*9.0-q2.*q4.*t5.*t6.*t8.*t16.*9.0-COM_X.*Jk.*Mb.*g.*t2.*t20.*8.0+COM_X.*Jk.*Mb.*g.*t3.*t20.*8.0-COM_X.*Jk.*Mb.*g.*t9.*t20.*8.0+COM_X.*Jk.*Mb.*g.*t10.*t20.*8.0-COM_X.*g.*t2.*t8.*t19.*t20.*8.0+COM_X.*g.*t3.*t8.*t19.*t20.*8.0-COM_X.*g.*t8.*t9.*t19.*t20.*8.0+COM_X.*g.*t8.*t10.*t19.*t20.*8.0-COM_X.*Mb.*Mk.*g.*t2.*t8.*t20.*8.0+COM_X.*Mb.*Mk.*g.*t3.*t8.*t20.*8.0-COM_X.*Mb.*Mk.*g.*t8.*t9.*t20.*8.0+COM_X.*Mb.*Mk.*g.*t8.*t10.*t20.*8.0-Jk.*Jw.*q1.*q3.*t2.*t6.*t18.*1.2e1+Jk.*Jw.*q1.*q3.*t3.*t6.*t18.*1.2e1-Jk.*Jw.*q2.*q4.*t2.*t6.*t18.*1.2e1+Jk.*Jw.*q2.*q4.*t3.*t6.*t18.*1.2e1+Jk.*Jw.*q1.*q3.*t6.*t9.*t18.*1.2e1-Jk.*Jw.*q1.*q3.*t6.*t10.*t18.*1.2e1+Jk.*Jw.*q2.*q4.*t6.*t9.*t18.*1.2e1-Jk.*Jw.*q2.*q4.*t6.*t10.*t18.*1.2e1+COM_Y.*g.*q1.*q4.*t8.*t19.*t20.*1.6e1-COM_Y.*g.*q2.*q3.*t8.*t19.*t20.*1.6e1-COM_Z.*g.*q1.*q3.*t8.*t19.*t20.*1.6e1-COM_Z.*g.*q2.*q4.*t8.*t19.*t20.*1.6e1-dx.*dy.*q1.*q2.*t5.*t8.*t13.*9.0+dx.*dy.*q1.*q2.*t5.*t8.*t14.*9.0+dx.*dy.*q1.*q2.*t5.*t8.*t15.*9.0-dx.*dy.*q1.*q2.*t5.*t8.*t16.*9.0+dx.*dy.*q3.*q4.*t5.*t8.*t13.*9.0-dx.*dy.*q3.*q4.*t5.*t8.*t14.*9.0-dx.*dy.*q3.*q4.*t5.*t8.*t15.*9.0+dx.*dy.*q3.*q4.*t5.*t8.*t16.*9.0+q1.*q3.*t2.*t4.*t5.*t6.*t8.*4.5e1+q1.*q3.*t3.*t4.*t5.*t6.*t8.*9.0+q2.*q4.*t2.*t4.*t5.*t6.*t8.*1.17e2-q1.*q3.*t2.*t5.*t6.*t7.*t8.*9.0+q2.*q4.*t3.*t4.*t5.*t6.*t8.*9.0-q1.*q3.*t3.*t5.*t6.*t7.*t8.*4.5e1-q2.*q4.*t2.*t5.*t6.*t7.*t8.*9.0-q2.*q4.*t3.*t5.*t6.*t7.*t8.*1.17e2-q1.*q3.*t2.*t5.*t6.*t8.*t11.*2.7e1+q1.*q3.*t4.*t5.*t6.*t8.*t9.*2.7e1+q1.*q3.*t2.*t5.*t6.*t8.*t12.*9.0-q1.*q3.*t3.*t5.*t6.*t8.*t11.*9.0+q1.*q3.*t4.*t5.*t6.*t8.*t10.*9.0+q1.*q3.*t3.*t5.*t6.*t8.*t12.*2.7e1-q2.*q4.*t2.*t5.*t6.*t8.*t11.*2.7e1+q2.*q4.*t4.*t5.*t6.*t8.*t9.*2.7e1-q1.*q3.*t4.*t5.*t6.*t8.*t12.*5.4e1-q1.*q3.*t5.*t6.*t7.*t8.*t9.*9.0+q2.*q4.*t2.*t5.*t6.*t8.*t12.*9.0-q2.*q4.*t3.*t5.*t6.*t8.*t11.*9.0+q2.*q4.*t4.*t5.*t6.*t8.*t10.*9.0-q1.*q3.*t5.*t6.*t7.*t8.*t10.*2.7e1+q2.*q4.*t3.*t5.*t6.*t8.*t12.*2.7e1+q1.*q3.*t5.*t6.*t7.*t8.*t11.*5.4e1-q2.*q4.*t4.*t5.*t6.*t8.*t12.*5.4e1-q2.*q4.*t5.*t6.*t7.*t8.*t9.*9.0-q2.*q4.*t2.*t4.*t5.*t8.*t17.*7.2e1-q2.*q4.*t5.*t6.*t7.*t8.*t10.*2.7e1-q1.*q3.*t5.*t6.*t8.*t9.*t11.*1.17e2+q2.*q4.*t5.*t6.*t7.*t8.*t11.*5.4e1+q1.*q3.*t5.*t6.*t8.*t9.*t12.*9.0-q1.*q3.*t5.*t6.*t8.*t10.*t11.*9.0+q1.*q3.*t5.*t6.*t8.*t10.*t12.*1.17e2-q2.*q4.*t5.*t6.*t8.*t9.*t11.*4.5e1+q2.*q4.*t3.*t5.*t7.*t8.*t17.*7.2e1+q2.*q4.*t5.*t6.*t8.*t9.*t12.*9.0-q2.*q4.*t5.*t6.*t8.*t10.*t11.*9.0+q2.*q4.*t5.*t6.*t8.*t10.*t12.*4.5e1+q1.*q3.*t2.*t5.*t8.*t12.*t17.*3.6e1-q1.*q3.*t3.*t5.*t8.*t11.*t17.*3.6e1+q1.*q3.*t4.*t5.*t8.*t10.*t17.*3.6e1-q1.*q3.*t5.*t7.*t8.*t9.*t17.*3.6e1+q2.*q4.*t2.*t5.*t8.*t12.*t17.*3.6e1-q2.*q4.*t3.*t5.*t8.*t11.*t17.*3.6e1+q2.*q4.*t4.*t5.*t8.*t10.*t17.*3.6e1-q2.*q4.*t5.*t7.*t8.*t9.*t17.*3.6e1+q1.*q3.*t5.*t8.*t9.*t11.*t17.*7.2e1-q1.*q3.*t5.*t8.*t10.*t12.*t17.*7.2e1+COM_Y.*Jk.*Mb.*g.*q1.*q4.*t20.*1.6e1-COM_Y.*Jk.*Mb.*g.*q2.*q3.*t20.*1.6e1-COM_Z.*Jk.*Mb.*g.*q1.*q3.*t20.*1.6e1-COM_Z.*Jk.*Mb.*g.*q2.*q4.*t20.*1.6e1-COM_X.*Jw.*Mb.*g.*t8.*t13.*t18.*6.0+COM_X.*Jw.*Mb.*g.*t8.*t14.*t18.*6.0-COM_X.*Jw.*Mb.*g.*t8.*t15.*t18.*6.0+COM_X.*Jw.*Mb.*g.*t8.*t16.*t18.*6.0+COM_Y.*Mb.*Mk.*g.*q1.*q4.*t8.*t20.*1.6e1-COM_Y.*Mb.*Mk.*g.*q2.*q3.*t8.*t20.*1.6e1-COM_Z.*Mb.*Mk.*g.*q1.*q3.*t8.*t20.*1.6e1-COM_Z.*Mb.*Mk.*g.*q2.*q4.*t8.*t20.*1.6e1+COM_X.*Jw.*Mb.*g.*t2.*t4.*t8.*t18.*3.0e1+COM_X.*Jw.*Mb.*g.*t3.*t4.*t8.*t18.*6.0-COM_X.*Jw.*Mb.*g.*t2.*t7.*t8.*t18.*6.0-COM_X.*Jw.*Mb.*g.*t3.*t7.*t8.*t18.*3.0e1+COM_X.*Jw.*Mb.*g.*t2.*t8.*t11.*t18.*6.0+COM_X.*Jw.*Mb.*g.*t4.*t8.*t9.*t18.*6.0-COM_X.*Jw.*Mb.*g.*t2.*t8.*t12.*t18.*1.8e1+COM_X.*Jw.*Mb.*g.*t3.*t8.*t11.*t18.*1.8e1+COM_X.*Jw.*Mb.*g.*t4.*t8.*t10.*t18.*1.8e1-COM_X.*Jw.*Mb.*g.*t3.*t8.*t12.*t18.*6.0-COM_X.*Jw.*Mb.*g.*t4.*t8.*t11.*t18.*3.6e1-COM_X.*Jw.*Mb.*g.*t7.*t8.*t9.*t18.*1.8e1-COM_X.*Jw.*Mb.*g.*t7.*t8.*t10.*t18.*6.0+COM_X.*Jw.*Mb.*g.*t7.*t8.*t12.*t18.*3.6e1+COM_X.*Jw.*Mb.*g.*t8.*t9.*t11.*t18.*3.0e1-COM_X.*Jw.*Mb.*g.*t8.*t9.*t12.*t18.*6.0+COM_X.*Jw.*Mb.*g.*t8.*t10.*t11.*t18.*6.0-COM_X.*Jw.*Mb.*g.*t8.*t10.*t12.*t18.*3.0e1-Jk.*Jw.*dx.*dy.*q1.*q2.*t2.*t18.*1.2e1+Jk.*Jw.*dx.*dy.*q1.*q2.*t3.*t18.*1.2e1+Jk.*Jw.*dx.*dy.*q3.*q4.*t2.*t18.*1.2e1-Jk.*Jw.*dx.*dy.*q3.*q4.*t3.*t18.*1.2e1+Jk.*Jw.*dx.*dy.*q1.*q2.*t9.*t18.*1.2e1-Jk.*Jw.*dx.*dy.*q1.*q2.*t10.*t18.*1.2e1-Jk.*Jw.*dx.*dy.*q3.*q4.*t9.*t18.*1.2e1+Jk.*Jw.*dx.*dy.*q3.*q4.*t10.*t18.*1.2e1+COM_X.*COM_Z.*g.*rk.*t2.*t4.*t19.*t20.*4.0e1-COM_X.*COM_Z.*g.*rk.*t3.*t4.*t19.*t20.*8.0-COM_X.*COM_Z.*g.*rk.*t2.*t7.*t19.*t20.*8.0+COM_X.*COM_Z.*g.*rk.*t3.*t7.*t19.*t20.*4.0e1+COM_X.*COM_Z.*g.*rk.*t2.*t11.*t19.*t20.*8.0-COM_X.*COM_Z.*g.*rk.*t4.*t9.*t19.*t20.*8.0+COM_X.*COM_Z.*g.*rk.*t2.*t12.*t19.*t20.*8.0+COM_X.*COM_Z.*g.*rk.*t3.*t11.*t19.*t20.*8.0-COM_X.*COM_Z.*g.*rk.*t4.*t10.*t19.*t20.*8.0+COM_X.*COM_Z.*g.*rk.*t3.*t12.*t19.*t20.*8.0-COM_X.*COM_Z.*g.*rk.*t7.*t9.*t19.*t20.*8.0-COM_X.*COM_Z.*g.*rk.*t7.*t10.*t19.*t20.*8.0-COM_X.*COM_Z.*g.*rk.*t9.*t11.*t19.*t20.*4.0e1+COM_X.*COM_Z.*g.*rk.*t9.*t12.*t19.*t20.*8.0+COM_X.*COM_Z.*g.*rk.*t10.*t11.*t19.*t20.*8.0-COM_X.*COM_Z.*g.*rk.*t10.*t12.*t19.*t20.*4.0e1-Jw.*Mb.*q1.*q3.*t2.*t6.*t8.*t18.*1.2e1+Jw.*Mb.*q1.*q3.*t3.*t6.*t8.*t18.*1.2e1-Jw.*Mb.*q2.*q4.*t2.*t6.*t8.*t18.*1.2e1+Jw.*Mb.*q2.*q4.*t3.*t6.*t8.*t18.*1.2e1+Jw.*Mb.*q1.*q3.*t6.*t8.*t9.*t18.*1.2e1-Jw.*Mb.*q1.*q3.*t6.*t8.*t10.*t18.*1.2e1+Jw.*Mb.*q2.*q4.*t6.*t8.*t9.*t18.*1.2e1-Jw.*Mb.*q2.*q4.*t6.*t8.*t10.*t18.*1.2e1-Jw.*Mk.*q1.*q3.*t2.*t6.*t8.*t18.*1.2e1+Jw.*Mk.*q1.*q3.*t3.*t6.*t8.*t18.*1.2e1-Jw.*Mk.*q2.*q4.*t2.*t6.*t8.*t18.*1.2e1+Jw.*Mk.*q2.*q4.*t3.*t6.*t8.*t18.*1.2e1+Jw.*Mk.*q1.*q3.*t6.*t8.*t9.*t18.*1.2e1-Jw.*Mk.*q1.*q3.*t6.*t8.*t10.*t18.*1.2e1+Jw.*Mk.*q2.*q4.*t6.*t8.*t9.*t18.*1.2e1-Jw.*Mk.*q2.*q4.*t6.*t8.*t10.*t18.*1.2e1+dx.*dy.*q1.*q2.*t2.*t4.*t5.*t8.*8.1e1+dx.*dy.*q1.*q2.*t3.*t4.*t5.*t8.*9.0-dx.*dy.*q1.*q2.*t2.*t5.*t7.*t8.*9.0-dx.*dy.*q1.*q2.*t3.*t5.*t7.*t8.*8.1e1-dx.*dy.*q3.*q4.*t2.*t4.*t5.*t8.*8.1e1-dx.*dy.*q3.*q4.*t3.*t4.*t5.*t8.*9.0-dx.*dy.*q1.*q2.*t2.*t5.*t8.*t11.*2.7e1+dx.*dy.*q1.*q2.*t4.*t5.*t8.*t9.*2.7e1+dx.*dy.*q3.*q4.*t2.*t5.*t7.*t8.*9.0+dx.*dy.*q1.*q2.*t2.*t5.*t8.*t12.*9.0+dx.*dy.*q1.*q2.*t3.*t5.*t8.*t11.*1.35e2-dx.*dy.*q1.*q2.*t4.*t5.*t8.*t10.*1.35e2+dx.*dy.*q3.*q4.*t3.*t5.*t7.*t8.*8.1e1+dx.*dy.*q1.*q2.*t3.*t5.*t8.*t12.*2.7e1-dx.*dy.*q1.*q2.*t4.*t5.*t8.*t12.*5.4e1-dx.*dy.*q1.*q2.*t5.*t7.*t8.*t9.*9.0-dx.*dy.*q1.*q2.*t5.*t7.*t8.*t10.*2.7e1+dx.*dy.*q3.*q4.*t2.*t5.*t8.*t11.*2.7e1-dx.*dy.*q3.*q4.*t4.*t5.*t8.*t9.*2.7e1+dx.*dy.*q1.*q2.*t5.*t7.*t8.*t11.*5.4e1+dx.*dy.*q3.*q4.*t2.*t5.*t8.*t12.*1.35e2+dx.*dy.*q3.*q4.*t3.*t5.*t8.*t11.*9.0-dx.*dy.*q3.*q4.*t4.*t5.*t8.*t10.*9.0-dx.*dy.*q3.*q4.*t3.*t5.*t8.*t12.*2.7e1-dx.*dy.*q1.*q2.*t5.*t8.*t9.*t11.*8.1e1+dx.*dy.*q3.*q4.*t4.*t5.*t8.*t12.*5.4e1-dx.*dy.*q3.*q4.*t5.*t7.*t8.*t9.*1.35e2+dx.*dy.*q1.*q2.*t5.*t8.*t9.*t12.*9.0-dx.*dy.*q1.*q2.*t5.*t8.*t10.*t11.*9.0+dx.*dy.*q3.*q4.*t5.*t7.*t8.*t10.*2.7e1+dx.*dy.*q1.*q2.*t5.*t8.*t10.*t12.*8.1e1-dx.*dy.*q3.*q4.*t5.*t7.*t8.*t11.*5.4e1+dx.*dy.*q3.*q4.*t5.*t8.*t9.*t11.*8.1e1-dx.*dy.*q3.*q4.*t5.*t8.*t9.*t12.*9.0+dx.*dy.*q3.*q4.*t5.*t8.*t10.*t11.*9.0-dx.*dy.*q3.*q4.*t5.*t8.*t10.*t12.*8.1e1-g.*q1.*q3.*rk.*t4.*t19.*t20.*t21.*1.6e1+g.*q1.*q3.*rk.*t4.*t19.*t20.*t22.*1.6e1+g.*q2.*q4.*rk.*t4.*t19.*t20.*t21.*1.6e1+g.*q1.*q3.*rk.*t7.*t19.*t20.*t21.*1.6e1+g.*q2.*q4.*rk.*t4.*t19.*t20.*t22.*1.6e1-g.*q1.*q3.*rk.*t7.*t19.*t20.*t22.*1.6e1-g.*q2.*q4.*rk.*t4.*t19.*t20.*t23.*3.2e1-g.*q2.*q4.*rk.*t7.*t19.*t20.*t21.*1.6e1-g.*q2.*q4.*rk.*t7.*t19.*t20.*t22.*1.6e1-g.*q1.*q3.*rk.*t11.*t19.*t20.*t21.*1.6e1+g.*q2.*q4.*rk.*t7.*t19.*t20.*t23.*3.2e1-g.*q1.*q3.*rk.*t11.*t19.*t20.*t22.*1.6e1+g.*q1.*q3.*rk.*t12.*t19.*t20.*t21.*1.6e1+g.*q1.*q3.*rk.*t11.*t19.*t20.*t23.*3.2e1+g.*q1.*q3.*rk.*t12.*t19.*t20.*t22.*1.6e1+g.*q2.*q4.*rk.*t11.*t19.*t20.*t21.*1.6e1-g.*q1.*q3.*rk.*t12.*t19.*t20.*t23.*3.2e1-g.*q2.*q4.*rk.*t11.*t19.*t20.*t22.*1.6e1-g.*q2.*q4.*rk.*t12.*t19.*t20.*t21.*1.6e1+g.*q2.*q4.*rk.*t12.*t19.*t20.*t22.*1.6e1-q1.*q3.*t2.*t3.*t4.*t5.*t6.*t8.*5.4e1-q2.*q4.*t2.*t3.*t4.*t5.*t6.*t8.*1.26e2+q1.*q3.*t2.*t3.*t5.*t6.*t7.*t8.*5.4e1+q1.*q3.*t2.*t3.*t5.*t6.*t8.*t9.*5.4e1+q2.*q4.*t2.*t3.*t5.*t6.*t7.*t8.*1.26e2-q1.*q3.*t2.*t3.*t5.*t6.*t8.*t10.*5.4e1-q1.*q3.*t2.*t4.*t5.*t6.*t8.*t9.*1.26e2+q1.*q3.*t2.*t3.*t5.*t6.*t8.*t11.*9.0e1-q1.*q3.*t3.*t4.*t5.*t6.*t8.*t9.*3.6e1+q2.*q4.*t2.*t3.*t5.*t6.*t8.*t9.*1.26e2-q1.*q3.*t2.*t3.*t5.*t6.*t8.*t12.*9.0e1-q1.*q3.*t3.*t4.*t5.*t6.*t8.*t10.*1.8e1-q2.*q4.*t2.*t3.*t5.*t6.*t8.*t10.*1.26e2-q2.*q4.*t2.*t4.*t5.*t6.*t8.*t9.*2.7e2+q1.*q3.*t2.*t5.*t6.*t7.*t8.*t9.*1.8e1+q2.*q4.*t2.*t3.*t5.*t6.*t8.*t11.*1.62e2-q2.*q4.*t3.*t4.*t5.*t6.*t8.*t9.*3.6e1+q1.*q3.*t2.*t5.*t6.*t7.*t8.*t10.*3.6e1-q2.*q4.*t2.*t3.*t5.*t6.*t8.*t12.*1.62e2-q2.*q4.*t3.*t4.*t5.*t6.*t8.*t10.*1.8e1+q1.*q3.*t3.*t5.*t6.*t7.*t8.*t10.*1.26e2+q2.*q4.*t2.*t5.*t6.*t7.*t8.*t9.*1.8e1-q1.*q3.*t2.*t5.*t6.*t8.*t9.*t10.*1.26e2+q2.*q4.*t2.*t5.*t6.*t7.*t8.*t10.*3.6e1+q1.*q3.*t2.*t5.*t6.*t8.*t9.*t11.*2.7e2+q1.*q3.*t3.*t5.*t6.*t8.*t9.*t10.*1.26e2+q2.*q4.*t2.*t3.*t4.*t5.*t8.*t17.*7.2e1+q2.*q4.*t3.*t5.*t6.*t7.*t8.*t10.*2.7e2-q1.*q3.*t2.*t5.*t6.*t8.*t9.*t12.*1.8e1+q1.*q3.*t2.*t5.*t6.*t8.*t10.*t11.*3.6e1-q1.*q3.*t4.*t5.*t6.*t8.*t9.*t10.*1.62e2-q2.*q4.*t2.*t5.*t6.*t8.*t9.*t10.*5.4e1-q1.*q3.*t3.*t5.*t6.*t8.*t9.*t12.*3.6e1+q1.*q3.*t3.*t5.*t6.*t8.*t10.*t11.*1.8e1+q2.*q4.*t2.*t5.*t6.*t8.*t9.*t11.*1.26e2+q2.*q4.*t3.*t5.*t6.*t8.*t9.*t10.*5.4e1+q1.*q3.*t2.*t3.*t5.*t8.*t9.*t17.*3.6e1-q1.*q3.*t3.*t5.*t6.*t8.*t10.*t12.*2.7e2-q2.*q4.*t2.*t3.*t5.*t7.*t8.*t17.*7.2e1-q2.*q4.*t2.*t5.*t6.*t8.*t9.*t12.*1.8e1+q2.*q4.*t2.*t5.*t6.*t8.*t10.*t11.*3.6e1-q2.*q4.*t4.*t5.*t6.*t8.*t9.*t10.*9.0e1-q1.*q3.*t2.*t3.*t5.*t8.*t10.*t17.*3.6e1-q1.*q3.*t2.*t4.*t5.*t8.*t9.*t17.*3.6e1+q1.*q3.*t5.*t6.*t7.*t8.*t9.*t10.*1.62e2-q2.*q4.*t3.*t5.*t6.*t8.*t9.*t12.*3.6e1+q2.*q4.*t3.*t5.*t6.*t8.*t10.*t11.*1.8e1+q1.*q3.*t2.*t3.*t5.*t8.*t11.*t17.*7.2e1-q2.*q4.*t2.*t3.*t5.*t8.*t9.*t17.*3.6e1-q2.*q4.*t3.*t5.*t6.*t8.*t10.*t12.*1.26e2-q1.*q3.*t2.*t3.*t5.*t8.*t12.*t17.*7.2e1-q1.*q3.*t3.*t4.*t5.*t8.*t10.*t17.*3.6e1+q2.*q4.*t2.*t3.*t5.*t8.*t10.*t17.*3.6e1+q2.*q4.*t2.*t4.*t5.*t8.*t9.*t17.*1.08e2+q2.*q4.*t5.*t6.*t7.*t8.*t9.*t10.*9.0e1+q1.*q3.*t2.*t5.*t7.*t8.*t9.*t17.*3.6e1+q1.*q3.*t5.*t6.*t8.*t9.*t10.*t11.*1.26e2-q2.*q4.*t3.*t4.*t5.*t8.*t10.*t17.*3.6e1+q1.*q3.*t3.*t5.*t7.*t8.*t10.*t17.*3.6e1-q1.*q3.*t5.*t6.*t8.*t9.*t10.*t12.*1.26e2+q2.*q4.*t2.*t5.*t7.*t8.*t9.*t17.*3.6e1+q1.*q3.*t2.*t5.*t8.*t9.*t10.*t17.*3.6e1+q2.*q4.*t5.*t6.*t8.*t9.*t10.*t11.*5.4e1-q1.*q3.*t2.*t5.*t8.*t9.*t11.*t17.*1.08e2-q1.*q3.*t3.*t5.*t8.*t9.*t10.*t17.*3.6e1-q2.*q4.*t3.*t5.*t7.*t8.*t10.*t17.*1.08e2-q2.*q4.*t5.*t6.*t8.*t9.*t10.*t12.*5.4e1-q1.*q3.*t2.*t5.*t8.*t9.*t12.*t17.*3.6e1-q2.*q4.*t2.*t5.*t8.*t9.*t10.*t17.*3.6e1+q1.*q3.*t3.*t5.*t8.*t10.*t11.*t17.*3.6e1+q2.*q4.*t2.*t5.*t8.*t9.*t11.*t17.*3.6e1+q2.*q4.*t3.*t5.*t8.*t9.*t10.*t17.*3.6e1+q1.*q3.*t3.*t5.*t8.*t10.*t12.*t17.*1.08e2-q2.*q4.*t2.*t5.*t8.*t9.*t12.*t17.*3.6e1-q2.*q4.*t4.*t5.*t8.*t9.*t10.*t17.*7.2e1+q2.*q4.*t3.*t5.*t8.*t10.*t11.*t17.*3.6e1-q2.*q4.*t3.*t5.*t8.*t10.*t12.*t17.*3.6e1+q2.*q4.*t5.*t7.*t8.*t9.*t10.*t17.*7.2e1-q1.*q3.*t5.*t8.*t9.*t10.*t11.*t17.*7.2e1+q1.*q3.*t5.*t8.*t9.*t10.*t12.*t17.*7.2e1+COM_X.*COM_Z.*g.*rk.*t2.*t3.*t9.*t19.*t20.*4.8e1+COM_X.*COM_Z.*g.*rk.*t2.*t3.*t10.*t19.*t20.*4.8e1-COM_X.*COM_Z.*g.*rk.*t2.*t9.*t10.*t19.*t20.*4.8e1-COM_X.*COM_Z.*g.*rk.*t3.*t9.*t10.*t19.*t20.*4.8e1-dx.*dy.*q1.*q2.*t2.*t3.*t4.*t5.*t8.*9.0e1+dx.*dy.*q1.*q2.*t2.*t3.*t5.*t7.*t8.*9.0e1+dx.*dy.*q3.*q4.*t2.*t3.*t4.*t5.*t8.*9.0e1+dx.*dy.*q1.*q2.*t2.*t3.*t5.*t8.*t9.*9.0e1+dx.*dy.*q1.*q2.*t2.*t3.*t5.*t8.*t10.*5.4e1-dx.*dy.*q1.*q2.*t2.*t4.*t5.*t8.*t9.*1.98e2-dx.*dy.*q1.*q2.*t2.*t3.*t5.*t8.*t11.*1.62e2-dx.*dy.*q1.*q2.*t3.*t4.*t5.*t8.*t9.*3.6e1-dx.*dy.*q3.*q4.*t2.*t3.*t5.*t7.*t8.*9.0e1-dx.*dy.*q1.*q2.*t2.*t3.*t5.*t8.*t12.*1.26e2+dx.*dy.*q1.*q2.*t3.*t4.*t5.*t8.*t10.*1.26e2+dx.*dy.*q1.*q2.*t2.*t5.*t7.*t8.*t9.*1.8e1+dx.*dy.*q3.*q4.*t2.*t3.*t5.*t8.*t9.*5.4e1+dx.*dy.*q1.*q2.*t2.*t5.*t7.*t8.*t10.*3.6e1+dx.*dy.*q3.*q4.*t2.*t3.*t5.*t8.*t10.*9.0e1+dx.*dy.*q3.*q4.*t2.*t4.*t5.*t8.*t9.*5.4e1+dx.*dy.*q1.*q2.*t3.*t5.*t7.*t8.*t10.*5.4e1-dx.*dy.*q3.*q4.*t2.*t3.*t5.*t8.*t11.*1.26e2+dx.*dy.*q3.*q4.*t3.*t4.*t5.*t8.*t9.*3.6e1-dx.*dy.*q1.*q2.*t2.*t5.*t8.*t9.*t10.*9.0e1-dx.*dy.*q3.*q4.*t2.*t3.*t5.*t8.*t12.*1.62e2+dx.*dy.*q3.*q4.*t3.*t4.*t5.*t8.*t10.*1.8e1+dx.*dy.*q1.*q2.*t2.*t5.*t8.*t9.*t11.*1.98e2-dx.*dy.*q1.*q2.*t3.*t5.*t8.*t9.*t10.*5.4e1+dx.*dy.*q3.*q4.*t2.*t5.*t7.*t8.*t9.*1.26e2-dx.*dy.*q1.*q2.*t2.*t5.*t8.*t9.*t12.*1.8e1+dx.*dy.*q1.*q2.*t2.*t5.*t8.*t10.*t11.*3.6e1+dx.*dy.*q1.*q2.*t4.*t5.*t8.*t9.*t10.*1.62e2-dx.*dy.*q3.*q4.*t2.*t5.*t7.*t8.*t10.*3.6e1-dx.*dy.*q1.*q2.*t3.*t5.*t8.*t9.*t12.*3.6e1-dx.*dy.*q1.*q2.*t3.*t5.*t8.*t10.*t11.*1.26e2-dx.*dy.*q3.*q4.*t3.*t5.*t7.*t8.*t10.*1.98e2-dx.*dy.*q1.*q2.*t3.*t5.*t8.*t10.*t12.*5.4e1-dx.*dy.*q3.*q4.*t2.*t5.*t8.*t9.*t10.*5.4e1+dx.*dy.*q1.*q2.*t5.*t7.*t8.*t9.*t10.*1.26e2-dx.*dy.*q3.*q4.*t2.*t5.*t8.*t9.*t11.*5.4e1-dx.*dy.*q3.*q4.*t3.*t5.*t8.*t9.*t10.*9.0e1-dx.*dy.*q3.*q4.*t2.*t5.*t8.*t9.*t12.*1.26e2-dx.*dy.*q3.*q4.*t2.*t5.*t8.*t10.*t11.*3.6e1+dx.*dy.*q3.*q4.*t4.*t5.*t8.*t9.*t10.*1.26e2+dx.*dy.*q3.*q4.*t3.*t5.*t8.*t9.*t12.*3.6e1-dx.*dy.*q3.*q4.*t3.*t5.*t8.*t10.*t11.*1.8e1+dx.*dy.*q3.*q4.*t3.*t5.*t8.*t10.*t12.*1.98e2+dx.*dy.*q1.*q2.*t5.*t8.*t9.*t10.*t11.*9.0e1+dx.*dy.*q3.*q4.*t5.*t7.*t8.*t9.*t10.*1.62e2-dx.*dy.*q1.*q2.*t5.*t8.*t9.*t10.*t12.*9.0e1-dx.*dy.*q3.*q4.*t5.*t8.*t9.*t10.*t11.*9.0e1+dx.*dy.*q3.*q4.*t5.*t8.*t9.*t10.*t12.*9.0e1+g.*q1.*q3.*rk.*t2.*t9.*t19.*t20.*t21.*3.2e1-g.*q1.*q3.*rk.*t2.*t9.*t19.*t20.*t23.*3.2e1-g.*q1.*q3.*rk.*t2.*t10.*t19.*t20.*t22.*3.2e1+g.*q1.*q3.*rk.*t3.*t9.*t19.*t20.*t22.*3.2e1-g.*q1.*q3.*rk.*t3.*t10.*t19.*t20.*t21.*3.2e1-g.*q2.*q4.*rk.*t2.*t9.*t19.*t20.*t21.*3.2e1+g.*q1.*q3.*rk.*t2.*t10.*t19.*t20.*t23.*3.2e1-g.*q1.*q3.*rk.*t3.*t9.*t19.*t20.*t23.*3.2e1+g.*q1.*q3.*rk.*t3.*t10.*t19.*t20.*t23.*3.2e1+g.*q2.*q4.*rk.*t2.*t9.*t19.*t20.*t23.*3.2e1-g.*q2.*q4.*rk.*t2.*t10.*t19.*t20.*t22.*3.2e1+g.*q2.*q4.*rk.*t3.*t9.*t19.*t20.*t22.*3.2e1+g.*q2.*q4.*rk.*t3.*t10.*t19.*t20.*t21.*3.2e1+g.*q2.*q4.*rk.*t2.*t10.*t19.*t20.*t23.*3.2e1-g.*q2.*q4.*rk.*t3.*t9.*t19.*t20.*t23.*3.2e1-g.*q2.*q4.*rk.*t3.*t10.*t19.*t20.*t23.*3.2e1-COM_Y.*Jw.*Mb.*g.*q1.*q4.*t4.*t8.*t18.*1.2e1-COM_Y.*Jw.*Mb.*g.*q2.*q3.*t4.*t8.*t18.*1.2e1-COM_Z.*Jw.*Mb.*g.*q1.*q3.*t4.*t8.*t18.*1.2e1-COM_Z.*Jw.*Mb.*g.*q2.*q4.*t4.*t8.*t18.*1.2e1+COM_Y.*Jw.*Mb.*g.*q1.*q4.*t7.*t8.*t18.*1.2e1+COM_Y.*Jw.*Mb.*g.*q2.*q3.*t7.*t8.*t18.*1.2e1-COM_Z.*Jw.*Mb.*g.*q1.*q3.*t7.*t8.*t18.*1.2e1-COM_Z.*Jw.*Mb.*g.*q2.*q4.*t7.*t8.*t18.*1.2e1+COM_Y.*Jw.*Mb.*g.*q1.*q4.*t8.*t11.*t18.*1.2e1+COM_Y.*Jw.*Mb.*g.*q2.*q3.*t8.*t11.*t18.*1.2e1-COM_Z.*Jw.*Mb.*g.*q1.*q3.*t8.*t11.*t18.*1.2e1-COM_Y.*Jw.*Mb.*g.*q1.*q4.*t8.*t12.*t18.*1.2e1-COM_Y.*Jw.*Mb.*g.*q2.*q3.*t8.*t12.*t18.*1.2e1-COM_Z.*Jw.*Mb.*g.*q1.*q3.*t8.*t12.*t18.*1.2e1-COM_Z.*Jw.*Mb.*g.*q2.*q4.*t8.*t11.*t18.*1.2e1-COM_Z.*Jw.*Mb.*g.*q2.*q4.*t8.*t12.*t18.*1.2e1-COM_X.*Jw.*Mb.*g.*t2.*t3.*t4.*t8.*t18.*3.6e1+COM_X.*Jw.*Mb.*g.*t2.*t3.*t7.*t8.*t18.*3.6e1+COM_X.*Jw.*Mb.*g.*t2.*t3.*t8.*t9.*t18.*1.2e1-COM_X.*Jw.*Mb.*g.*t2.*t3.*t8.*t10.*t18.*1.2e1-COM_X.*Jw.*Mb.*g.*t2.*t4.*t8.*t9.*t18.*4.8e1-COM_X.*Jw.*Mb.*g.*t2.*t3.*t8.*t11.*t18.*1.2e1-COM_X.*Jw.*Mb.*g.*t2.*t4.*t8.*t10.*t18.*3.6e1-COM_X.*Jw.*Mb.*g.*t3.*t4.*t8.*t9.*t18.*1.2e1+COM_X.*Jw.*Mb.*g.*t2.*t3.*t8.*t12.*t18.*1.2e1-COM_X.*Jw.*Mb.*g.*t3.*t4.*t8.*t10.*t18.*2.4e1+COM_X.*Jw.*Mb.*g.*t2.*t7.*t8.*t9.*t18.*2.4e1+COM_X.*Jw.*Mb.*g.*t2.*t7.*t8.*t10.*t18.*1.2e1+COM_X.*Jw.*Mb.*g.*t3.*t7.*t8.*t9.*t18.*3.6e1+COM_X.*Jw.*Mb.*g.*t3.*t7.*t8.*t10.*t18.*4.8e1+COM_X.*Jw.*Mb.*g.*t2.*t8.*t9.*t10.*t18.*1.2e1-COM_X.*Jw.*Mb.*g.*t2.*t8.*t9.*t11.*t18.*4.8e1-COM_X.*Jw.*Mb.*g.*t3.*t8.*t9.*t10.*t18.*1.2e1+COM_X.*Jw.*Mb.*g.*t2.*t8.*t9.*t12.*t18.*2.4e1-COM_X.*Jw.*Mb.*g.*t2.*t8.*t10.*t11.*t18.*1.2e1-COM_X.*Jw.*Mb.*g.*t3.*t8.*t9.*t11.*t18.*3.6e1-COM_X.*Jw.*Mb.*g.*t4.*t8.*t9.*t10.*t18.*1.2e1+COM_X.*Jw.*Mb.*g.*t2.*t8.*t10.*t12.*t18.*3.6e1+COM_X.*Jw.*Mb.*g.*t3.*t8.*t9.*t12.*t18.*1.2e1-COM_X.*Jw.*Mb.*g.*t3.*t8.*t10.*t11.*t18.*2.4e1+COM_X.*Jw.*Mb.*g.*t3.*t8.*t10.*t12.*t18.*4.8e1+COM_X.*Jw.*Mb.*g.*t7.*t8.*t9.*t10.*t18.*1.2e1-COM_X.*Jw.*Mb.*g.*t8.*t9.*t10.*t11.*t18.*3.6e1+COM_X.*Jw.*Mb.*g.*t8.*t9.*t10.*t12.*t18.*3.6e1-COM_X.*Jw.*Mb.*rk.*t2.*t3.*t4.*t6.*t18.*2.4e1+COM_X.*Jw.*Mb.*rk.*t2.*t3.*t6.*t7.*t18.*2.4e1-COM_X.*Jw.*Mb.*rk.*t2.*t4.*t6.*t9.*t18.*4.8e1+COM_X.*Jw.*Mb.*rk.*t2.*t3.*t6.*t11.*t18.*2.4e1-COM_X.*Jw.*Mb.*rk.*t2.*t3.*t6.*t12.*t18.*2.4e1+COM_X.*Jw.*Mb.*rk.*t3.*t6.*t7.*t10.*t18.*4.8e1-COM_X.*Jw.*Mb.*rk.*t2.*t6.*t9.*t11.*t18.*4.8e1+COM_X.*Jw.*Mb.*rk.*t4.*t6.*t9.*t10.*t18.*2.4e1+COM_X.*Jw.*Mb.*rk.*t3.*t6.*t10.*t12.*t18.*4.8e1-COM_X.*Jw.*Mb.*rk.*t6.*t7.*t9.*t10.*t18.*2.4e1-COM_X.*Jw.*Mb.*rk.*t6.*t9.*t10.*t11.*t18.*2.4e1+COM_X.*Jw.*Mb.*rk.*t6.*t9.*t10.*t12.*t18.*2.4e1+COM_X.*COM_Y.*g.*q1.*q2.*rk.*t4.*t19.*t20.*4.8e1-COM_X.*COM_Y.*g.*q1.*q2.*rk.*t7.*t19.*t20.*1.6e1+COM_X.*COM_Y.*g.*q3.*q4.*rk.*t4.*t19.*t20.*1.6e1-COM_Y.*COM_Z.*g.*q1.*q4.*rk.*t4.*t19.*t20.*4.8e1+COM_Y.*COM_Z.*g.*q2.*q3.*rk.*t4.*t19.*t20.*1.6e1+COM_X.*COM_Y.*g.*q1.*q2.*rk.*t11.*t19.*t20.*4.8e1-COM_X.*COM_Y.*g.*q3.*q4.*rk.*t7.*t19.*t20.*4.8e1+COM_Y.*COM_Z.*g.*q1.*q4.*rk.*t7.*t19.*t20.*1.6e1-COM_Y.*COM_Z.*g.*q2.*q3.*rk.*t7.*t19.*t20.*4.8e1-COM_X.*COM_Y.*g.*q1.*q2.*rk.*t12.*t19.*t20.*1.6e1+COM_X.*COM_Y.*g.*q3.*q4.*rk.*t11.*t19.*t20.*1.6e1+COM_Y.*COM_Z.*g.*q1.*q4.*rk.*t11.*t19.*t20.*1.6e1-COM_Y.*COM_Z.*g.*q2.*q3.*rk.*t11.*t19.*t20.*4.8e1-COM_X.*COM_Y.*g.*q3.*q4.*rk.*t12.*t19.*t20.*4.8e1-COM_Y.*COM_Z.*g.*q1.*q4.*rk.*t12.*t19.*t20.*4.8e1+COM_Y.*COM_Z.*g.*q2.*q3.*rk.*t12.*t19.*t20.*1.6e1-Jw.*Mb.*dx.*dy.*q1.*q2.*t2.*t8.*t18.*1.2e1+Jw.*Mb.*dx.*dy.*q1.*q2.*t3.*t8.*t18.*1.2e1+Jw.*Mb.*dx.*dy.*q3.*q4.*t2.*t8.*t18.*1.2e1-Jw.*Mb.*dx.*dy.*q3.*q4.*t3.*t8.*t18.*1.2e1+Jw.*Mb.*dx.*dy.*q1.*q2.*t8.*t9.*t18.*1.2e1-Jw.*Mb.*dx.*dy.*q1.*q2.*t8.*t10.*t18.*1.2e1-Jw.*Mb.*dx.*dy.*q3.*q4.*t8.*t9.*t18.*1.2e1+Jw.*Mb.*dx.*dy.*q3.*q4.*t8.*t10.*t18.*1.2e1-Jw.*Mk.*dx.*dy.*q1.*q2.*t2.*t8.*t18.*1.2e1+Jw.*Mk.*dx.*dy.*q1.*q2.*t3.*t8.*t18.*1.2e1+Jw.*Mk.*dx.*dy.*q3.*q4.*t2.*t8.*t18.*1.2e1-Jw.*Mk.*dx.*dy.*q3.*q4.*t3.*t8.*t18.*1.2e1+Jw.*Mk.*dx.*dy.*q1.*q2.*t8.*t9.*t18.*1.2e1-Jw.*Mk.*dx.*dy.*q1.*q2.*t8.*t10.*t18.*1.2e1-Jw.*Mk.*dx.*dy.*q3.*q4.*t8.*t9.*t18.*1.2e1+Jw.*Mk.*dx.*dy.*q3.*q4.*t8.*t10.*t18.*1.2e1+COM_Y.*Jw.*Mb.*dx.*dy.*rk.*t2.*t4.*t9.*t18.*2.4e1+COM_Y.*Jw.*Mb.*dx.*dy.*rk.*t2.*t3.*t11.*t18.*4.8e1+COM_Y.*Jw.*Mb.*dx.*dy.*rk.*t2.*t3.*t12.*t18.*4.8e1-COM_Y.*Jw.*Mb.*dx.*dy.*rk.*t3.*t4.*t10.*t18.*2.4e1-COM_Y.*Jw.*Mb.*dx.*dy.*rk.*t2.*t7.*t9.*t18.*2.4e1+COM_Y.*Jw.*Mb.*dx.*dy.*rk.*t3.*t7.*t10.*t18.*2.4e1-COM_Y.*Jw.*Mb.*dx.*dy.*rk.*t2.*t9.*t11.*t18.*2.4e1+COM_Y.*Jw.*Mb.*dx.*dy.*rk.*t2.*t9.*t12.*t18.*2.4e1-COM_Y.*Jw.*Mb.*dx.*dy.*rk.*t4.*t9.*t10.*t18.*4.8e1+COM_Y.*Jw.*Mb.*dx.*dy.*rk.*t3.*t10.*t11.*t18.*2.4e1-COM_Y.*Jw.*Mb.*dx.*dy.*rk.*t3.*t10.*t12.*t18.*2.4e1-COM_Y.*Jw.*Mb.*dx.*dy.*rk.*t7.*t9.*t10.*t18.*4.8e1+COM_Y.*Jw.*Mb.*g.*q1.*q4.*t2.*t3.*t8.*t18.*4.8e1-COM_Y.*Jw.*Mb.*g.*q2.*q3.*t2.*t3.*t8.*t18.*4.8e1-COM_Z.*Jw.*Mb.*g.*q1.*q3.*t2.*t3.*t8.*t18.*7.2e1-COM_Y.*Jw.*Mb.*g.*q1.*q4.*t2.*t4.*t8.*t18.*3.6e1+COM_Y.*Jw.*Mb.*g.*q2.*q3.*t2.*t4.*t8.*t18.*6.0e1+COM_Z.*Jw.*Mb.*g.*q1.*q3.*t2.*t4.*t8.*t18.*8.4e1+COM_Y.*Jw.*Mb.*g.*q1.*q4.*t3.*t4.*t8.*t18.*1.2e1+COM_Y.*Jw.*Mb.*g.*q2.*q3.*t3.*t4.*t8.*t18.*1.2e1+COM_Z.*Jw.*Mb.*g.*q1.*q3.*t3.*t4.*t8.*t18.*1.2e1-COM_Z.*Jw.*Mb.*g.*q2.*q4.*t2.*t3.*t8.*t18.*7.2e1+COM_Z.*Jw.*Mb.*g.*q2.*q4.*t2.*t4.*t8.*t18.*8.4e1-COM_Y.*Jw.*Mb.*g.*q1.*q4.*t2.*t7.*t8.*t18.*1.2e1-COM_Y.*Jw.*Mb.*g.*q2.*q3.*t2.*t7.*t8.*t18.*1.2e1+COM_Z.*Jw.*Mb.*g.*q1.*q3.*t2.*t7.*t8.*t18.*1.2e1+COM_Z.*Jw.*Mb.*g.*q2.*q4.*t3.*t4.*t8.*t18.*1.2e1-COM_Y.*Jw.*Mb.*g.*q1.*q4.*t3.*t7.*t8.*t18.*6.0e1+COM_Y.*Jw.*Mb.*g.*q2.*q3.*t3.*t7.*t8.*t18.*3.6e1+COM_Z.*Jw.*Mb.*g.*q1.*q3.*t3.*t7.*t8.*t18.*8.4e1+COM_Y.*Jw.*Mb.*g.*q1.*q4.*t2.*t8.*t9.*t18.*4.8e1-COM_Y.*Jw.*Mb.*g.*q2.*q3.*t2.*t8.*t9.*t18.*4.8e1-COM_Z.*Jw.*Mb.*g.*q1.*q3.*t2.*t8.*t9.*t18.*7.2e1+COM_Z.*Jw.*Mb.*g.*q2.*q4.*t2.*t7.*t8.*t18.*1.2e1+COM_Y.*Jw.*Mb.*g.*q1.*q4.*t2.*t8.*t10.*t18.*2.4e1-COM_Y.*Jw.*Mb.*g.*q1.*q4.*t3.*t8.*t9.*t18.*2.4e1+COM_Y.*Jw.*Mb.*g.*q2.*q3.*t2.*t8.*t10.*t18.*2.4e1-COM_Y.*Jw.*Mb.*g.*q2.*q3.*t3.*t8.*t9.*t18.*2.4e1-COM_Z.*Jw.*Mb.*g.*q1.*q3.*t2.*t8.*t10.*t18.*2.4e1-COM_Z.*Jw.*Mb.*g.*q1.*q3.*t3.*t8.*t9.*t18.*2.4e1+COM_Z.*Jw.*Mb.*g.*q2.*q4.*t3.*t7.*t8.*t18.*8.4e1-COM_Y.*Jw.*Mb.*g.*q1.*q4.*t2.*t8.*t11.*t18.*3.6e1+COM_Y.*Jw.*Mb.*g.*q1.*q4.*t3.*t8.*t10.*t18.*4.8e1+COM_Y.*Jw.*Mb.*g.*q1.*q4.*t4.*t8.*t9.*t18.*3.6e1-COM_Y.*Jw.*Mb.*g.*q2.*q3.*t2.*t8.*t11.*t18.*3.6e1-COM_Y.*Jw.*Mb.*g.*q2.*q3.*t3.*t8.*t10.*t18.*4.8e1+COM_Y.*Jw.*Mb.*g.*q2.*q3.*t4.*t8.*t9.*t18.*3.6e1+COM_Z.*Jw.*Mb.*g.*q1.*q3.*t2.*t8.*t11.*t18.*3.6e1-COM_Z.*Jw.*Mb.*g.*q1.*q3.*t3.*t8.*t10.*t18.*7.2e1+COM_Z.*Jw.*Mb.*g.*q1.*q3.*t4.*t8.*t9.*t18.*3.6e1-COM_Z.*Jw.*Mb.*g.*q2.*q4.*t2.*t8.*t9.*t18.*7.2e1-COM_Y.*Jw.*Mb.*g.*q1.*q4.*t2.*t8.*t12.*t18.*3.6e1-COM_Y.*Jw.*Mb.*g.*q1.*q4.*t3.*t8.*t11.*t18.*6.0e1-COM_Y.*Jw.*Mb.*g.*q1.*q4.*t4.*t8.*t10.*t18.*3.6e1+COM_Y.*Jw.*Mb.*g.*q2.*q3.*t2.*t8.*t12.*t18.*6.0e1+COM_Y.*Jw.*Mb.*g.*q2.*q3.*t3.*t8.*t11.*t18.*3.6e1+COM_Y.*Jw.*Mb.*g.*q2.*q3.*t4.*t8.*t10.*t18.*6.0e1+COM_Z.*Jw.*Mb.*g.*q1.*q3.*t2.*t8.*t12.*t18.*8.4e1+COM_Z.*Jw.*Mb.*g.*q1.*q3.*t3.*t8.*t11.*t18.*8.4e1+COM_Z.*Jw.*Mb.*g.*q1.*q3.*t4.*t8.*t10.*t18.*8.4e1-COM_Z.*Jw.*Mb.*g.*q2.*q4.*t2.*t8.*t10.*t18.*2.4e1-COM_Z.*Jw.*Mb.*g.*q2.*q4.*t3.*t8.*t9.*t18.*2.4e1+COM_Y.*Jw.*Mb.*g.*q1.*q4.*t3.*t8.*t12.*t18.*3.6e1+COM_Y.*Jw.*Mb.*g.*q2.*q3.*t3.*t8.*t12.*t18.*3.6e1+COM_Z.*Jw.*Mb.*g.*q1.*q3.*t3.*t8.*t12.*t18.*3.6e1+COM_Z.*Jw.*Mb.*g.*q2.*q4.*t2.*t8.*t11.*t18.*3.6e1-COM_Z.*Jw.*Mb.*g.*q2.*q4.*t3.*t8.*t10.*t18.*7.2e1+COM_Z.*Jw.*Mb.*g.*q2.*q4.*t4.*t8.*t9.*t18.*3.6e1-COM_Y.*Jw.*Mb.*g.*q1.*q4.*t7.*t8.*t9.*t18.*6.0e1+COM_Y.*Jw.*Mb.*g.*q2.*q3.*t7.*t8.*t9.*t18.*3.6e1+COM_Z.*Jw.*Mb.*g.*q1.*q3.*t7.*t8.*t9.*t18.*8.4e1+COM_Z.*Jw.*Mb.*g.*q2.*q4.*t2.*t8.*t12.*t18.*8.4e1+COM_Z.*Jw.*Mb.*g.*q2.*q4.*t3.*t8.*t11.*t18.*8.4e1+COM_Z.*Jw.*Mb.*g.*q2.*q4.*t4.*t8.*t10.*t18.*8.4e1-COM_Y.*Jw.*Mb.*g.*q1.*q4.*t7.*t8.*t10.*t18.*3.6e1-COM_Y.*Jw.*Mb.*g.*q2.*q3.*t7.*t8.*t10.*t18.*3.6e1+COM_Z.*Jw.*Mb.*g.*q1.*q3.*t7.*t8.*t10.*t18.*3.6e1+COM_Z.*Jw.*Mb.*g.*q2.*q4.*t3.*t8.*t12.*t18.*3.6e1+COM_Z.*Jw.*Mb.*g.*q2.*q4.*t7.*t8.*t9.*t18.*8.4e1+COM_Y.*Jw.*Mb.*g.*q1.*q4.*t8.*t9.*t10.*t18.*4.8e1-COM_Y.*Jw.*Mb.*g.*q2.*q3.*t8.*t9.*t10.*t18.*4.8e1-COM_Z.*Jw.*Mb.*g.*q1.*q3.*t8.*t9.*t10.*t18.*7.2e1+COM_Z.*Jw.*Mb.*g.*q2.*q4.*t7.*t8.*t10.*t18.*3.6e1-COM_Y.*Jw.*Mb.*g.*q1.*q4.*t8.*t9.*t11.*t18.*6.0e1+COM_Y.*Jw.*Mb.*g.*q2.*q3.*t8.*t9.*t11.*t18.*3.6e1+COM_Z.*Jw.*Mb.*g.*q1.*q3.*t8.*t9.*t11.*t18.*8.4e1+COM_Y.*Jw.*Mb.*g.*q1.*q4.*t8.*t9.*t12.*t18.*1.2e1-COM_Y.*Jw.*Mb.*g.*q1.*q4.*t8.*t10.*t11.*t18.*1.2e1+COM_Y.*Jw.*Mb.*g.*q2.*q3.*t8.*t9.*t12.*t18.*1.2e1-COM_Y.*Jw.*Mb.*g.*q2.*q3.*t8.*t10.*t11.*t18.*1.2e1+COM_Z.*Jw.*Mb.*g.*q1.*q3.*t8.*t9.*t12.*t18.*1.2e1+COM_Z.*Jw.*Mb.*g.*q1.*q3.*t8.*t10.*t11.*t18.*1.2e1-COM_Z.*Jw.*Mb.*g.*q2.*q4.*t8.*t9.*t10.*t18.*7.2e1-COM_Y.*Jw.*Mb.*g.*q1.*q4.*t8.*t10.*t12.*t18.*3.6e1+COM_Y.*Jw.*Mb.*g.*q2.*q3.*t8.*t10.*t12.*t18.*6.0e1+COM_Z.*Jw.*Mb.*g.*q1.*q3.*t8.*t10.*t12.*t18.*8.4e1+COM_Z.*Jw.*Mb.*g.*q2.*q4.*t8.*t9.*t11.*t18.*8.4e1+COM_Z.*Jw.*Mb.*g.*q2.*q4.*t8.*t9.*t12.*t18.*1.2e1+COM_Z.*Jw.*Mb.*g.*q2.*q4.*t8.*t10.*t11.*t18.*1.2e1+COM_Z.*Jw.*Mb.*g.*q2.*q4.*t8.*t10.*t12.*t18.*8.4e1+COM_Y.*Jw.*Mb.*q2.*q3.*rk.*t2.*t4.*t6.*t18.*2.4e1+COM_Z.*Jw.*Mb.*q1.*q3.*rk.*t2.*t4.*t6.*t18.*1.2e1+COM_Y.*Jw.*Mb.*q1.*q4.*rk.*t3.*t4.*t6.*t18.*2.4e1-COM_Z.*Jw.*Mb.*q1.*q3.*rk.*t3.*t4.*t6.*t18.*1.2e1+COM_Z.*Jw.*Mb.*q2.*q4.*rk.*t2.*t4.*t6.*t18.*1.2e1-COM_Y.*Jw.*Mb.*q2.*q3.*rk.*t2.*t6.*t7.*t18.*2.4e1-COM_Z.*Jw.*Mb.*q1.*q3.*rk.*t2.*t6.*t7.*t18.*1.2e1-COM_Z.*Jw.*Mb.*q2.*q4.*rk.*t3.*t4.*t6.*t18.*1.2e1-COM_Y.*Jw.*Mb.*q1.*q4.*rk.*t3.*t6.*t7.*t18.*2.4e1+COM_Z.*Jw.*Mb.*q1.*q3.*rk.*t3.*t6.*t7.*t18.*1.2e1-COM_Z.*Jw.*Mb.*q2.*q4.*rk.*t2.*t6.*t7.*t18.*1.2e1+COM_Z.*Jw.*Mb.*q2.*q4.*rk.*t3.*t6.*t7.*t18.*1.2e1+COM_Y.*Jw.*Mb.*q1.*q4.*rk.*t4.*t6.*t9.*t18.*7.2e1-COM_Y.*Jw.*Mb.*q2.*q3.*rk.*t2.*t6.*t11.*t18.*7.2e1-COM_Z.*Jw.*Mb.*q1.*q3.*rk.*t2.*t6.*t11.*t18.*3.6e1-COM_Z.*Jw.*Mb.*q1.*q3.*rk.*t4.*t6.*t9.*t18.*3.6e1-COM_Y.*Jw.*Mb.*q1.*q4.*rk.*t3.*t6.*t11.*t18.*2.4e1+COM_Y.*Jw.*Mb.*q2.*q3.*rk.*t2.*t6.*t12.*t18.*2.4e1+COM_Y.*Jw.*Mb.*q2.*q3.*rk.*t4.*t6.*t10.*t18.*2.4e1+COM_Z.*Jw.*Mb.*q1.*q3.*rk.*t2.*t6.*t12.*t18.*1.2e1+COM_Z.*Jw.*Mb.*q1.*q3.*rk.*t3.*t6.*t11.*t18.*1.2e1+COM_Z.*Jw.*Mb.*q1.*q3.*rk.*t4.*t6.*t10.*t18.*1.2e1+COM_Y.*Jw.*Mb.*q1.*q4.*rk.*t3.*t6.*t12.*t18.*7.2e1-COM_Z.*Jw.*Mb.*q1.*q3.*rk.*t3.*t6.*t12.*t18.*3.6e1-COM_Z.*Jw.*Mb.*q2.*q4.*rk.*t2.*t6.*t11.*t18.*3.6e1-COM_Z.*Jw.*Mb.*q2.*q4.*rk.*t4.*t6.*t9.*t18.*3.6e1-COM_Y.*Jw.*Mb.*q1.*q4.*rk.*t6.*t7.*t9.*t18.*2.4e1+COM_Z.*Jw.*Mb.*q1.*q3.*rk.*t6.*t7.*t9.*t18.*1.2e1+COM_Z.*Jw.*Mb.*q2.*q4.*rk.*t2.*t6.*t12.*t18.*1.2e1+COM_Z.*Jw.*Mb.*q2.*q4.*rk.*t3.*t6.*t11.*t18.*1.2e1+COM_Z.*Jw.*Mb.*q2.*q4.*rk.*t4.*t6.*t10.*t18.*1.2e1-COM_Y.*Jw.*Mb.*q2.*q3.*rk.*t6.*t7.*t10.*t18.*7.2e1-COM_Z.*Jw.*Mb.*q1.*q3.*rk.*t6.*t7.*t10.*t18.*3.6e1-COM_Z.*Jw.*Mb.*q2.*q4.*rk.*t3.*t6.*t12.*t18.*3.6e1+COM_Z.*Jw.*Mb.*q2.*q4.*rk.*t6.*t7.*t9.*t18.*1.2e1-COM_Z.*Jw.*Mb.*q2.*q4.*rk.*t6.*t7.*t10.*t18.*3.6e1-COM_Y.*Jw.*Mb.*q1.*q4.*rk.*t6.*t9.*t11.*t18.*2.4e1+COM_Z.*Jw.*Mb.*q1.*q3.*rk.*t6.*t9.*t11.*t18.*1.2e1+COM_Y.*Jw.*Mb.*q1.*q4.*rk.*t6.*t9.*t12.*t18.*2.4e1-COM_Y.*Jw.*Mb.*q2.*q3.*rk.*t6.*t10.*t11.*t18.*2.4e1-COM_Z.*Jw.*Mb.*q1.*q3.*rk.*t6.*t9.*t12.*t18.*1.2e1-COM_Z.*Jw.*Mb.*q1.*q3.*rk.*t6.*t10.*t11.*t18.*1.2e1+COM_Y.*Jw.*Mb.*q2.*q3.*rk.*t6.*t10.*t12.*t18.*2.4e1+COM_Z.*Jw.*Mb.*q1.*q3.*rk.*t6.*t10.*t12.*t18.*1.2e1+COM_Z.*Jw.*Mb.*q2.*q4.*rk.*t6.*t9.*t11.*t18.*1.2e1-COM_Z.*Jw.*Mb.*q2.*q4.*rk.*t6.*t9.*t12.*t18.*1.2e1-COM_Z.*Jw.*Mb.*q2.*q4.*rk.*t6.*t10.*t11.*t18.*1.2e1+COM_Z.*Jw.*Mb.*q2.*q4.*rk.*t6.*t10.*t12.*t18.*1.2e1+COM_X.*COM_Y.*g.*q1.*q2.*rk.*t2.*t3.*t19.*t20.*3.2e1-COM_X.*COM_Y.*g.*q3.*q4.*rk.*t2.*t3.*t19.*t20.*3.2e1-COM_Y.*COM_Z.*g.*q1.*q4.*rk.*t2.*t3.*t19.*t20.*3.2e1-COM_Y.*COM_Z.*g.*q2.*q3.*rk.*t2.*t3.*t19.*t20.*3.2e1-COM_X.*COM_Y.*g.*q1.*q2.*rk.*t2.*t9.*t19.*t20.*3.2e1+COM_X.*COM_Y.*g.*q1.*q2.*rk.*t2.*t10.*t19.*t20.*3.2e1+COM_X.*COM_Y.*g.*q1.*q2.*rk.*t3.*t9.*t19.*t20.*3.2e1+COM_X.*COM_Y.*g.*q1.*q2.*rk.*t3.*t10.*t19.*t20.*9.6e1-COM_X.*COM_Y.*g.*q3.*q4.*rk.*t2.*t9.*t19.*t20.*9.6e1-COM_Y.*COM_Z.*g.*q1.*q4.*rk.*t2.*t9.*t19.*t20.*3.2e1-COM_Y.*COM_Z.*g.*q2.*q3.*rk.*t2.*t9.*t19.*t20.*3.2e1-COM_X.*COM_Y.*g.*q3.*q4.*rk.*t2.*t10.*t19.*t20.*3.2e1-COM_X.*COM_Y.*g.*q3.*q4.*rk.*t3.*t9.*t19.*t20.*3.2e1+COM_Y.*COM_Z.*g.*q1.*q4.*rk.*t2.*t10.*t19.*t20.*3.2e1-COM_Y.*COM_Z.*g.*q1.*q4.*rk.*t3.*t9.*t19.*t20.*9.6e1-COM_Y.*COM_Z.*g.*q2.*q3.*rk.*t2.*t10.*t19.*t20.*9.6e1+COM_Y.*COM_Z.*g.*q2.*q3.*rk.*t3.*t9.*t19.*t20.*3.2e1+COM_X.*COM_Y.*g.*q3.*q4.*rk.*t3.*t10.*t19.*t20.*3.2e1-COM_Y.*COM_Z.*g.*q1.*q4.*rk.*t3.*t10.*t19.*t20.*3.2e1-COM_Y.*COM_Z.*g.*q2.*q3.*rk.*t3.*t10.*t19.*t20.*3.2e1+COM_X.*COM_Y.*g.*q1.*q2.*rk.*t9.*t10.*t19.*t20.*3.2e1-COM_X.*COM_Y.*g.*q3.*q4.*rk.*t9.*t10.*t19.*t20.*3.2e1-COM_Y.*COM_Z.*g.*q1.*q4.*rk.*t9.*t10.*t19.*t20.*3.2e1-COM_Y.*COM_Z.*g.*q2.*q3.*rk.*t9.*t10.*t19.*t20.*3.2e1-COM_X.*Jw.*Mb.*dx.*dy.*q2.*q3.*rk.*t2.*t4.*t18.*2.4e1+COM_Z.*Jw.*Mb.*dx.*dy.*q1.*q2.*rk.*t2.*t4.*t18.*1.2e1+COM_X.*Jw.*Mb.*dx.*dy.*q1.*q4.*rk.*t3.*t4.*t18.*2.4e1-COM_Z.*Jw.*Mb.*dx.*dy.*q1.*q2.*rk.*t3.*t4.*t18.*1.2e1+COM_X.*Jw.*Mb.*dx.*dy.*q2.*q3.*rk.*t2.*t7.*t18.*2.4e1-COM_Z.*Jw.*Mb.*dx.*dy.*q1.*q2.*rk.*t2.*t7.*t18.*1.2e1-COM_X.*Jw.*Mb.*dx.*dy.*q1.*q4.*rk.*t3.*t7.*t18.*2.4e1+COM_Z.*Jw.*Mb.*dx.*dy.*q1.*q2.*rk.*t3.*t7.*t18.*1.2e1-COM_Z.*Jw.*Mb.*dx.*dy.*q3.*q4.*rk.*t2.*t4.*t18.*1.2e1+COM_Z.*Jw.*Mb.*dx.*dy.*q3.*q4.*rk.*t3.*t4.*t18.*1.2e1+COM_X.*Jw.*Mb.*dx.*dy.*q1.*q4.*rk.*t4.*t9.*t18.*7.2e1+COM_X.*Jw.*Mb.*dx.*dy.*q2.*q3.*rk.*t2.*t11.*t18.*7.2e1-COM_Z.*Jw.*Mb.*dx.*dy.*q1.*q2.*rk.*t2.*t11.*t18.*3.6e1-COM_Z.*Jw.*Mb.*dx.*dy.*q1.*q2.*rk.*t4.*t9.*t18.*3.6e1+COM_Z.*Jw.*Mb.*dx.*dy.*q3.*q4.*rk.*t2.*t7.*t18.*1.2e1-COM_X.*Jw.*Mb.*dx.*dy.*q1.*q4.*rk.*t3.*t11.*t18.*2.4e1-COM_X.*Jw.*Mb.*dx.*dy.*q2.*q3.*rk.*t2.*t12.*t18.*2.4e1-COM_X.*Jw.*Mb.*dx.*dy.*q2.*q3.*rk.*t4.*t10.*t18.*2.4e1+COM_Z.*Jw.*Mb.*dx.*dy.*q1.*q2.*rk.*t2.*t12.*t18.*1.2e1+COM_Z.*Jw.*Mb.*dx.*dy.*q1.*q2.*rk.*t3.*t11.*t18.*1.2e1+COM_Z.*Jw.*Mb.*dx.*dy.*q1.*q2.*rk.*t4.*t10.*t18.*1.2e1-COM_Z.*Jw.*Mb.*dx.*dy.*q3.*q4.*rk.*t3.*t7.*t18.*1.2e1+COM_X.*Jw.*Mb.*dx.*dy.*q1.*q4.*rk.*t3.*t12.*t18.*7.2e1-COM_Z.*Jw.*Mb.*dx.*dy.*q1.*q2.*rk.*t3.*t12.*t18.*3.6e1-COM_X.*Jw.*Mb.*dx.*dy.*q1.*q4.*rk.*t7.*t9.*t18.*2.4e1+COM_Z.*Jw.*Mb.*dx.*dy.*q1.*q2.*rk.*t7.*t9.*t18.*1.2e1+COM_X.*Jw.*Mb.*dx.*dy.*q2.*q3.*rk.*t7.*t10.*t18.*7.2e1-COM_Z.*Jw.*Mb.*dx.*dy.*q1.*q2.*rk.*t7.*t10.*t18.*3.6e1+COM_Z.*Jw.*Mb.*dx.*dy.*q3.*q4.*rk.*t2.*t11.*t18.*3.6e1+COM_Z.*Jw.*Mb.*dx.*dy.*q3.*q4.*rk.*t4.*t9.*t18.*3.6e1-COM_Z.*Jw.*Mb.*dx.*dy.*q3.*q4.*rk.*t2.*t12.*t18.*1.2e1-COM_Z.*Jw.*Mb.*dx.*dy.*q3.*q4.*rk.*t3.*t11.*t18.*1.2e1-COM_Z.*Jw.*Mb.*dx.*dy.*q3.*q4.*rk.*t4.*t10.*t18.*1.2e1+COM_Z.*Jw.*Mb.*dx.*dy.*q3.*q4.*rk.*t3.*t12.*t18.*3.6e1-COM_X.*Jw.*Mb.*dx.*dy.*q1.*q4.*rk.*t9.*t11.*t18.*2.4e1+COM_Z.*Jw.*Mb.*dx.*dy.*q1.*q2.*rk.*t9.*t11.*t18.*1.2e1-COM_Z.*Jw.*Mb.*dx.*dy.*q3.*q4.*rk.*t7.*t9.*t18.*1.2e1+COM_X.*Jw.*Mb.*dx.*dy.*q1.*q4.*rk.*t9.*t12.*t18.*2.4e1+COM_X.*Jw.*Mb.*dx.*dy.*q2.*q3.*rk.*t10.*t11.*t18.*2.4e1-COM_Z.*Jw.*Mb.*dx.*dy.*q1.*q2.*rk.*t9.*t12.*t18.*1.2e1-COM_Z.*Jw.*Mb.*dx.*dy.*q1.*q2.*rk.*t10.*t11.*t18.*1.2e1+COM_Z.*Jw.*Mb.*dx.*dy.*q3.*q4.*rk.*t7.*t10.*t18.*3.6e1-COM_X.*Jw.*Mb.*dx.*dy.*q2.*q3.*rk.*t10.*t12.*t18.*2.4e1+COM_Z.*Jw.*Mb.*dx.*dy.*q1.*q2.*rk.*t10.*t12.*t18.*1.2e1-COM_Z.*Jw.*Mb.*dx.*dy.*q3.*q4.*rk.*t9.*t11.*t18.*1.2e1+COM_Z.*Jw.*Mb.*dx.*dy.*q3.*q4.*rk.*t9.*t12.*t18.*1.2e1+COM_Z.*Jw.*Mb.*dx.*dy.*q3.*q4.*rk.*t10.*t11.*t18.*1.2e1-COM_Z.*Jw.*Mb.*dx.*dy.*q3.*q4.*rk.*t10.*t12.*t18.*1.2e1-COM_Y.*Jw.*Mb.*g.*q1.*q4.*t2.*t3.*t8.*t9.*t18.*7.2e1+COM_Y.*Jw.*Mb.*g.*q2.*q3.*t2.*t3.*t8.*t9.*t18.*1.2e2+COM_Z.*Jw.*Mb.*g.*q1.*q3.*t2.*t3.*t8.*t9.*t18.*1.68e2-COM_Y.*Jw.*Mb.*g.*q1.*q4.*t2.*t3.*t8.*t10.*t18.*1.2e2+COM_Y.*Jw.*Mb.*g.*q2.*q3.*t2.*t3.*t8.*t10.*t18.*7.2e1+COM_Z.*Jw.*Mb.*g.*q1.*q3.*t2.*t3.*t8.*t10.*t18.*1.68e2+COM_Z.*Jw.*Mb.*g.*q2.*q4.*t2.*t3.*t8.*t9.*t18.*1.68e2+COM_Z.*Jw.*Mb.*g.*q2.*q4.*t2.*t3.*t8.*t10.*t18.*1.68e2-COM_Y.*Jw.*Mb.*g.*q1.*q4.*t2.*t8.*t9.*t10.*t18.*1.2e2+COM_Y.*Jw.*Mb.*g.*q2.*q3.*t2.*t8.*t9.*t10.*t18.*7.2e1+COM_Z.*Jw.*Mb.*g.*q1.*q3.*t2.*t8.*t9.*t10.*t18.*1.68e2-COM_Y.*Jw.*Mb.*g.*q1.*q4.*t3.*t8.*t9.*t10.*t18.*7.2e1+COM_Y.*Jw.*Mb.*g.*q2.*q3.*t3.*t8.*t9.*t10.*t18.*1.2e2+COM_Z.*Jw.*Mb.*g.*q1.*q3.*t3.*t8.*t9.*t10.*t18.*1.68e2+COM_Z.*Jw.*Mb.*g.*q2.*q4.*t2.*t8.*t9.*t10.*t18.*1.68e2+COM_Z.*Jw.*Mb.*g.*q2.*q4.*t3.*t8.*t9.*t10.*t18.*1.68e2+COM_Y.*Jw.*Mb.*q2.*q3.*rk.*t2.*t3.*t6.*t9.*t18.*4.8e1+COM_Z.*Jw.*Mb.*q1.*q3.*rk.*t2.*t3.*t6.*t9.*t18.*2.4e1-COM_Y.*Jw.*Mb.*q1.*q4.*rk.*t2.*t3.*t6.*t10.*t18.*4.8e1+COM_Z.*Jw.*Mb.*q1.*q3.*rk.*t2.*t3.*t6.*t10.*t18.*2.4e1+COM_Z.*Jw.*Mb.*q2.*q4.*rk.*t2.*t3.*t6.*t9.*t18.*2.4e1+COM_Z.*Jw.*Mb.*q2.*q4.*rk.*t2.*t3.*t6.*t10.*t18.*2.4e1-COM_Y.*Jw.*Mb.*q1.*q4.*rk.*t2.*t6.*t9.*t10.*t18.*4.8e1+COM_Z.*Jw.*Mb.*q1.*q3.*rk.*t2.*t6.*t9.*t10.*t18.*2.4e1+COM_Y.*Jw.*Mb.*q2.*q3.*rk.*t3.*t6.*t9.*t10.*t18.*4.8e1+COM_Z.*Jw.*Mb.*q1.*q3.*rk.*t3.*t6.*t9.*t10.*t18.*2.4e1+COM_Z.*Jw.*Mb.*q2.*q4.*rk.*t2.*t6.*t9.*t10.*t18.*2.4e1+COM_Z.*Jw.*Mb.*q2.*q4.*rk.*t3.*t6.*t9.*t10.*t18.*2.4e1-COM_X.*Jw.*Mb.*dx.*dy.*q2.*q3.*rk.*t2.*t3.*t9.*t18.*4.8e1+COM_Z.*Jw.*Mb.*dx.*dy.*q1.*q2.*rk.*t2.*t3.*t9.*t18.*2.4e1-COM_X.*Jw.*Mb.*dx.*dy.*q1.*q4.*rk.*t2.*t3.*t10.*t18.*4.8e1+COM_Z.*Jw.*Mb.*dx.*dy.*q1.*q2.*rk.*t2.*t3.*t10.*t18.*2.4e1-COM_Z.*Jw.*Mb.*dx.*dy.*q3.*q4.*rk.*t2.*t3.*t9.*t18.*2.4e1-COM_Z.*Jw.*Mb.*dx.*dy.*q3.*q4.*rk.*t2.*t3.*t10.*t18.*2.4e1-COM_X.*Jw.*Mb.*dx.*dy.*q1.*q4.*rk.*t2.*t9.*t10.*t18.*4.8e1+COM_Z.*Jw.*Mb.*dx.*dy.*q1.*q2.*rk.*t2.*t9.*t10.*t18.*2.4e1-COM_X.*Jw.*Mb.*dx.*dy.*q2.*q3.*rk.*t3.*t9.*t10.*t18.*4.8e1+COM_Z.*Jw.*Mb.*dx.*dy.*q1.*q2.*rk.*t3.*t9.*t10.*t18.*2.4e1-COM_Z.*Jw.*Mb.*dx.*dy.*q3.*q4.*rk.*t2.*t9.*t10.*t18.*2.4e1-COM_Z.*Jw.*Mb.*dx.*dy.*q3.*q4.*rk.*t3.*t9.*t10.*t18.*2.4e1).*-2.0;rk.*t476.*(q1.*q2.*t5.*t8.*t13.*t17.*-9.0+q1.*q2.*t5.*t8.*t14.*t17.*9.0+q1.*q2.*t5.*t8.*t15.*t17.*9.0-q1.*q2.*t5.*t8.*t16.*t17.*9.0+q3.*q4.*t5.*t8.*t13.*t17.*9.0-q3.*q4.*t5.*t8.*t14.*t17.*9.0-q3.*q4.*t5.*t8.*t15.*t17.*9.0+q3.*q4.*t5.*t8.*t16.*t17.*9.0+COM_Y.*Jk.*Mb.*g.*t2.*t20.*8.0+COM_Y.*Jk.*Mb.*g.*t3.*t20.*8.0-COM_Y.*Jk.*Mb.*g.*t9.*t20.*8.0-COM_Y.*Jk.*Mb.*g.*t10.*t20.*8.0+COM_Y.*g.*t2.*t8.*t19.*t20.*8.0+COM_Y.*g.*t3.*t8.*t19.*t20.*8.0-COM_Y.*g.*t8.*t9.*t19.*t20.*8.0-COM_Y.*g.*t8.*t10.*t19.*t20.*8.0+COM_Y.*Mb.*Mk.*g.*t2.*t8.*t20.*8.0+COM_Y.*Mb.*Mk.*g.*t3.*t8.*t20.*8.0-COM_Y.*Mb.*Mk.*g.*t8.*t9.*t20.*8.0-COM_Y.*Mb.*Mk.*g.*t8.*t10.*t20.*8.0-Jk.*Jw.*q1.*q2.*t2.*t17.*t18.*1.2e1+Jk.*Jw.*q1.*q2.*t3.*t17.*t18.*1.2e1+Jk.*Jw.*q3.*q4.*t2.*t17.*t18.*1.2e1-Jk.*Jw.*q3.*q4.*t3.*t17.*t18.*1.2e1+Jk.*Jw.*q1.*q2.*t9.*t17.*t18.*1.2e1-Jk.*Jw.*q1.*q2.*t10.*t17.*t18.*1.2e1-Jk.*Jw.*q3.*q4.*t9.*t17.*t18.*1.2e1+Jk.*Jw.*q3.*q4.*t10.*t17.*t18.*1.2e1+COM_X.*g.*q1.*q4.*t8.*t19.*t20.*1.6e1+COM_X.*g.*q2.*q3.*t8.*t19.*t20.*1.6e1-COM_Z.*g.*q1.*q2.*t8.*t19.*t20.*1.6e1+COM_Z.*g.*q3.*q4.*t8.*t19.*t20.*1.6e1-dx.*dy.*q1.*q3.*t5.*t8.*t13.*9.0+dx.*dy.*q1.*q3.*t5.*t8.*t14.*9.0+dx.*dy.*q1.*q3.*t5.*t8.*t15.*9.0-dx.*dy.*q2.*q4.*t5.*t8.*t13.*9.0-dx.*dy.*q1.*q3.*t5.*t8.*t16.*9.0+dx.*dy.*q2.*q4.*t5.*t8.*t14.*9.0+dx.*dy.*q2.*q4.*t5.*t8.*t15.*9.0-dx.*dy.*q2.*q4.*t5.*t8.*t16.*9.0+q1.*q2.*t2.*t4.*t5.*t6.*t8.*3.6e1-q1.*q2.*t3.*t5.*t6.*t7.*t8.*3.6e1-q3.*q4.*t2.*t4.*t5.*t6.*t8.*3.6e1+q1.*q2.*t3.*t5.*t6.*t8.*t11.*7.2e1-q1.*q2.*t4.*t5.*t6.*t8.*t10.*7.2e1+q3.*q4.*t3.*t5.*t6.*t7.*t8.*3.6e1+q1.*q2.*t2.*t4.*t5.*t8.*t17.*9.0+q1.*q2.*t3.*t4.*t5.*t8.*t17.*9.0+q3.*q4.*t2.*t5.*t6.*t8.*t12.*7.2e1-q1.*q2.*t2.*t5.*t7.*t8.*t17.*9.0-q1.*q2.*t5.*t6.*t8.*t9.*t11.*3.6e1-q3.*q4.*t5.*t6.*t7.*t8.*t9.*7.2e1-q1.*q2.*t3.*t5.*t7.*t8.*t17.*9.0-q3.*q4.*t2.*t4.*t5.*t8.*t17.*9.0+q1.*q2.*t5.*t6.*t8.*t10.*t12.*3.6e1-q3.*q4.*t3.*t4.*t5.*t8.*t17.*9.0-q1.*q2.*t2.*t5.*t8.*t11.*t17.*2.7e1+q1.*q2.*t4.*t5.*t8.*t9.*t17.*2.7e1+q3.*q4.*t2.*t5.*t7.*t8.*t17.*9.0+q3.*q4.*t5.*t6.*t8.*t9.*t11.*3.6e1+q1.*q2.*t2.*t5.*t8.*t12.*t17.*4.5e1-q1.*q2.*t3.*t5.*t8.*t11.*t17.*1.17e2+q1.*q2.*t4.*t5.*t8.*t10.*t17.*1.17e2+q3.*q4.*t3.*t5.*t7.*t8.*t17.*9.0+q1.*q2.*t3.*t5.*t8.*t12.*t17.*2.7e1-q3.*q4.*t5.*t6.*t8.*t10.*t12.*3.6e1-q1.*q2.*t4.*t5.*t8.*t12.*t17.*5.4e1-q1.*q2.*t5.*t7.*t8.*t9.*t17.*4.5e1-q1.*q2.*t5.*t7.*t8.*t10.*t17.*2.7e1+q3.*q4.*t2.*t5.*t8.*t11.*t17.*2.7e1-q3.*q4.*t4.*t5.*t8.*t9.*t17.*2.7e1+q1.*q2.*t5.*t7.*t8.*t11.*t17.*5.4e1-q3.*q4.*t2.*t5.*t8.*t12.*t17.*1.17e2+q3.*q4.*t3.*t5.*t8.*t11.*t17.*4.5e1-q3.*q4.*t4.*t5.*t8.*t10.*t17.*4.5e1-q3.*q4.*t3.*t5.*t8.*t12.*t17.*2.7e1-q1.*q2.*t5.*t8.*t9.*t11.*t17.*9.0+q3.*q4.*t4.*t5.*t8.*t12.*t17.*5.4e1+q3.*q4.*t5.*t7.*t8.*t9.*t17.*1.17e2+q1.*q2.*t5.*t8.*t9.*t12.*t17.*9.0-q1.*q2.*t5.*t8.*t10.*t11.*t17.*9.0+q3.*q4.*t5.*t7.*t8.*t10.*t17.*2.7e1+q1.*q2.*t5.*t8.*t10.*t12.*t17.*9.0-q3.*q4.*t5.*t7.*t8.*t11.*t17.*5.4e1+q3.*q4.*t5.*t8.*t9.*t11.*t17.*9.0-q3.*q4.*t5.*t8.*t9.*t12.*t17.*9.0+q3.*q4.*t5.*t8.*t10.*t11.*t17.*9.0-q3.*q4.*t5.*t8.*t10.*t12.*t17.*9.0+COM_X.*Jk.*Mb.*g.*q1.*q4.*t20.*1.6e1+COM_X.*Jk.*Mb.*g.*q2.*q3.*t20.*1.6e1-COM_Z.*Jk.*Mb.*g.*q1.*q2.*t20.*1.6e1+COM_Z.*Jk.*Mb.*g.*q3.*q4.*t20.*1.6e1-COM_Y.*Jw.*Mb.*g.*t8.*t13.*t18.*6.0-COM_Y.*Jw.*Mb.*g.*t8.*t14.*t18.*6.0+COM_Y.*Jw.*Mb.*g.*t8.*t15.*t18.*6.0+COM_Y.*Jw.*Mb.*g.*t8.*t16.*t18.*6.0+COM_X.*Mb.*Mk.*g.*q1.*q4.*t8.*t20.*1.6e1+COM_X.*Mb.*Mk.*g.*q2.*q3.*t8.*t20.*1.6e1-COM_Z.*Mb.*Mk.*g.*q1.*q2.*t8.*t20.*1.6e1+COM_Z.*Mb.*Mk.*g.*q3.*q4.*t8.*t20.*1.6e1+COM_Y.*Jw.*Mb.*g.*t2.*t4.*t8.*t18.*1.8e1+COM_Y.*Jw.*Mb.*g.*t3.*t4.*t8.*t18.*6.0+COM_Y.*Jw.*Mb.*g.*t2.*t7.*t8.*t18.*6.0+COM_Y.*Jw.*Mb.*g.*t3.*t7.*t8.*t18.*1.8e1-COM_Y.*Jw.*Mb.*g.*t4.*t7.*t8.*t18.*3.6e1-COM_Y.*Jw.*Mb.*g.*t2.*t8.*t11.*t18.*6.0+COM_Y.*Jw.*Mb.*g.*t4.*t8.*t9.*t18.*6.0-COM_Y.*Jw.*Mb.*g.*t2.*t8.*t12.*t18.*3.0e1-COM_Y.*Jw.*Mb.*g.*t3.*t8.*t11.*t18.*3.0e1+COM_Y.*Jw.*Mb.*g.*t4.*t8.*t10.*t18.*3.0e1-COM_Y.*Jw.*Mb.*g.*t3.*t8.*t12.*t18.*6.0+COM_Y.*Jw.*Mb.*g.*t7.*t8.*t9.*t18.*3.0e1+COM_Y.*Jw.*Mb.*g.*t7.*t8.*t10.*t18.*6.0-COM_Y.*Jw.*Mb.*g.*t8.*t9.*t11.*t18.*1.8e1-COM_Y.*Jw.*Mb.*g.*t8.*t9.*t12.*t18.*6.0-COM_Y.*Jw.*Mb.*g.*t8.*t10.*t11.*t18.*6.0-COM_Y.*Jw.*Mb.*g.*t8.*t10.*t12.*t18.*1.8e1+COM_Y.*Jw.*Mb.*g.*t8.*t11.*t12.*t18.*3.6e1-Jk.*Jw.*dx.*dy.*q1.*q3.*t2.*t18.*1.2e1+Jk.*Jw.*dx.*dy.*q1.*q3.*t3.*t18.*1.2e1-Jk.*Jw.*dx.*dy.*q2.*q4.*t2.*t18.*1.2e1+Jk.*Jw.*dx.*dy.*q2.*q4.*t3.*t18.*1.2e1+Jk.*Jw.*dx.*dy.*q1.*q3.*t9.*t18.*1.2e1-Jk.*Jw.*dx.*dy.*q1.*q3.*t10.*t18.*1.2e1+Jk.*Jw.*dx.*dy.*q2.*q4.*t9.*t18.*1.2e1-Jk.*Jw.*dx.*dy.*q2.*q4.*t10.*t18.*1.2e1-COM_Y.*COM_Z.*g.*rk.*t2.*t4.*t19.*t20.*8.0-COM_Y.*COM_Z.*g.*rk.*t3.*t4.*t19.*t20.*8.0+COM_Y.*COM_Z.*g.*rk.*t2.*t7.*t19.*t20.*8.0+COM_Y.*COM_Z.*g.*rk.*t3.*t7.*t19.*t20.*8.0-COM_Y.*COM_Z.*g.*rk.*t2.*t11.*t19.*t20.*8.0-COM_Y.*COM_Z.*g.*rk.*t4.*t9.*t19.*t20.*8.0-COM_Y.*COM_Z.*g.*rk.*t2.*t12.*t19.*t20.*4.0e1+COM_Y.*COM_Z.*g.*rk.*t3.*t11.*t19.*t20.*4.0e1+COM_Y.*COM_Z.*g.*rk.*t4.*t10.*t19.*t20.*4.0e1+COM_Y.*COM_Z.*g.*rk.*t3.*t12.*t19.*t20.*8.0-COM_Y.*COM_Z.*g.*rk.*t7.*t9.*t19.*t20.*4.0e1+COM_Y.*COM_Z.*g.*rk.*t7.*t10.*t19.*t20.*8.0-COM_Y.*COM_Z.*g.*rk.*t9.*t11.*t19.*t20.*8.0+COM_Y.*COM_Z.*g.*rk.*t9.*t12.*t19.*t20.*8.0-COM_Y.*COM_Z.*g.*rk.*t10.*t11.*t19.*t20.*8.0+COM_Y.*COM_Z.*g.*rk.*t10.*t12.*t19.*t20.*8.0-Jw.*Mb.*q1.*q2.*t2.*t8.*t17.*t18.*1.2e1+Jw.*Mb.*q1.*q2.*t3.*t8.*t17.*t18.*1.2e1+Jw.*Mb.*q3.*q4.*t2.*t8.*t17.*t18.*1.2e1-Jw.*Mb.*q3.*q4.*t3.*t8.*t17.*t18.*1.2e1+Jw.*Mb.*q1.*q2.*t8.*t9.*t17.*t18.*1.2e1-Jw.*Mb.*q1.*q2.*t8.*t10.*t17.*t18.*1.2e1-Jw.*Mb.*q3.*q4.*t8.*t9.*t17.*t18.*1.2e1+Jw.*Mb.*q3.*q4.*t8.*t10.*t17.*t18.*1.2e1-Jw.*Mk.*q1.*q2.*t2.*t8.*t17.*t18.*1.2e1+Jw.*Mk.*q1.*q2.*t3.*t8.*t17.*t18.*1.2e1+Jw.*Mk.*q3.*q4.*t2.*t8.*t17.*t18.*1.2e1-Jw.*Mk.*q3.*q4.*t3.*t8.*t17.*t18.*1.2e1+Jw.*Mk.*q1.*q2.*t8.*t9.*t17.*t18.*1.2e1-Jw.*Mk.*q1.*q2.*t8.*t10.*t17.*t18.*1.2e1-Jw.*Mk.*q3.*q4.*t8.*t9.*t17.*t18.*1.2e1+Jw.*Mk.*q3.*q4.*t8.*t10.*t17.*t18.*1.2e1+dx.*dy.*q1.*q3.*t2.*t4.*t5.*t8.*9.0+dx.*dy.*q1.*q3.*t3.*t4.*t5.*t8.*9.0-dx.*dy.*q2.*q4.*t2.*t4.*t5.*t8.*1.35e2-dx.*dy.*q1.*q3.*t2.*t5.*t7.*t8.*9.0+dx.*dy.*q2.*q4.*t3.*t4.*t5.*t8.*9.0-dx.*dy.*q1.*q3.*t3.*t5.*t7.*t8.*9.0-dx.*dy.*q2.*q4.*t2.*t5.*t7.*t8.*9.0+dx.*dy.*q2.*q4.*t3.*t5.*t7.*t8.*1.35e2-dx.*dy.*q1.*q3.*t2.*t5.*t8.*t11.*2.7e1+dx.*dy.*q1.*q3.*t4.*t5.*t8.*t9.*2.7e1+dx.*dy.*q1.*q3.*t2.*t5.*t8.*t12.*8.1e1-dx.*dy.*q1.*q3.*t3.*t5.*t8.*t11.*8.1e1+dx.*dy.*q1.*q3.*t4.*t5.*t8.*t10.*8.1e1+dx.*dy.*q1.*q3.*t3.*t5.*t8.*t12.*2.7e1-dx.*dy.*q2.*q4.*t2.*t5.*t8.*t11.*2.7e1+dx.*dy.*q2.*q4.*t4.*t5.*t8.*t9.*2.7e1-dx.*dy.*q1.*q3.*t4.*t5.*t8.*t12.*5.4e1-dx.*dy.*q1.*q3.*t5.*t7.*t8.*t9.*8.1e1+dx.*dy.*q2.*q4.*t2.*t5.*t8.*t12.*8.1e1-dx.*dy.*q2.*q4.*t3.*t5.*t8.*t11.*8.1e1+dx.*dy.*q2.*q4.*t4.*t5.*t8.*t10.*8.1e1-dx.*dy.*q1.*q3.*t5.*t7.*t8.*t10.*2.7e1+dx.*dy.*q2.*q4.*t3.*t5.*t8.*t12.*2.7e1+dx.*dy.*q1.*q3.*t5.*t7.*t8.*t11.*5.4e1-dx.*dy.*q2.*q4.*t4.*t5.*t8.*t12.*5.4e1-dx.*dy.*q2.*q4.*t5.*t7.*t8.*t9.*8.1e1-dx.*dy.*q2.*q4.*t5.*t7.*t8.*t10.*2.7e1+dx.*dy.*q1.*q3.*t5.*t8.*t9.*t11.*1.35e2+dx.*dy.*q2.*q4.*t5.*t7.*t8.*t11.*5.4e1+dx.*dy.*q1.*q3.*t5.*t8.*t9.*t12.*9.0-dx.*dy.*q1.*q3.*t5.*t8.*t10.*t11.*9.0-dx.*dy.*q1.*q3.*t5.*t8.*t10.*t12.*1.35e2-dx.*dy.*q2.*q4.*t5.*t8.*t9.*t11.*9.0+dx.*dy.*q2.*q4.*t5.*t8.*t9.*t12.*9.0-dx.*dy.*q2.*q4.*t5.*t8.*t10.*t11.*9.0+dx.*dy.*q2.*q4.*t5.*t8.*t10.*t12.*9.0-g.*q1.*q2.*rk.*t4.*t19.*t20.*t21.*3.2e1+g.*q1.*q2.*rk.*t4.*t19.*t20.*t22.*1.6e1+g.*q1.*q2.*rk.*t4.*t19.*t20.*t23.*1.6e1-g.*q1.*q2.*rk.*t7.*t19.*t20.*t22.*1.6e1+g.*q1.*q2.*rk.*t7.*t19.*t20.*t23.*1.6e1-g.*q3.*q4.*rk.*t4.*t19.*t20.*t22.*1.6e1+g.*q3.*q4.*rk.*t4.*t19.*t20.*t23.*1.6e1+g.*q1.*q2.*rk.*t11.*t19.*t20.*t21.*3.2e1-g.*q3.*q4.*rk.*t7.*t19.*t20.*t21.*3.2e1-g.*q1.*q2.*rk.*t11.*t19.*t20.*t22.*1.6e1+g.*q3.*q4.*rk.*t7.*t19.*t20.*t22.*1.6e1-g.*q1.*q2.*rk.*t11.*t19.*t20.*t23.*1.6e1+g.*q1.*q2.*rk.*t12.*t19.*t20.*t22.*1.6e1+g.*q3.*q4.*rk.*t7.*t19.*t20.*t23.*1.6e1-g.*q1.*q2.*rk.*t12.*t19.*t20.*t23.*1.6e1+g.*q3.*q4.*rk.*t11.*t19.*t20.*t22.*1.6e1+g.*q3.*q4.*rk.*t12.*t19.*t20.*t21.*3.2e1-g.*q3.*q4.*rk.*t11.*t19.*t20.*t23.*1.6e1-g.*q3.*q4.*rk.*t12.*t19.*t20.*t22.*1.6e1-g.*q3.*q4.*rk.*t12.*t19.*t20.*t23.*1.6e1-q1.*q2.*t2.*t3.*t4.*t5.*t6.*t8.*3.6e1+q1.*q2.*t2.*t3.*t5.*t6.*t7.*t8.*3.6e1+q3.*q4.*t2.*t3.*t4.*t5.*t6.*t8.*3.6e1+q1.*q2.*t2.*t3.*t5.*t6.*t8.*t9.*3.6e1+q1.*q2.*t2.*t3.*t5.*t6.*t8.*t10.*3.6e1-q1.*q2.*t2.*t4.*t5.*t6.*t8.*t9.*7.2e1-q1.*q2.*t2.*t3.*t5.*t6.*t8.*t11.*1.08e2-q3.*q4.*t2.*t3.*t5.*t6.*t7.*t8.*3.6e1-q1.*q2.*t2.*t3.*t5.*t6.*t8.*t12.*3.6e1+q1.*q2.*t3.*t4.*t5.*t6.*t8.*t10.*7.2e1+q3.*q4.*t2.*t3.*t5.*t6.*t8.*t9.*3.6e1+q3.*q4.*t2.*t3.*t5.*t6.*t8.*t10.*3.6e1-q1.*q2.*t2.*t3.*t4.*t5.*t8.*t17.*1.8e1-q3.*q4.*t2.*t3.*t5.*t6.*t8.*t11.*3.6e1-q1.*q2.*t2.*t5.*t6.*t8.*t9.*t10.*3.6e1-q3.*q4.*t2.*t3.*t5.*t6.*t8.*t12.*1.08e2+q1.*q2.*t2.*t5.*t6.*t8.*t9.*t11.*7.2e1-q1.*q2.*t3.*t5.*t6.*t8.*t9.*t10.*3.6e1+q3.*q4.*t2.*t5.*t6.*t7.*t8.*t9.*7.2e1+q1.*q2.*t2.*t3.*t5.*t7.*t8.*t17.*1.8e1+q1.*q2.*t4.*t5.*t6.*t8.*t9.*t10.*1.08e2-q1.*q2.*t3.*t5.*t6.*t8.*t10.*t11.*7.2e1+q3.*q4.*t2.*t3.*t4.*t5.*t8.*t17.*1.8e1-q3.*q4.*t3.*t5.*t6.*t7.*t8.*t10.*7.2e1+q1.*q2.*t2.*t3.*t5.*t8.*t9.*t17.*5.4e1-q3.*q4.*t2.*t5.*t6.*t8.*t9.*t10.*3.6e1-q1.*q2.*t2.*t3.*t5.*t8.*t10.*t17.*1.26e2-q1.*q2.*t2.*t4.*t5.*t8.*t9.*t17.*9.0e1+q1.*q2.*t5.*t6.*t7.*t8.*t9.*t10.*3.6e1-q3.*q4.*t3.*t5.*t6.*t8.*t9.*t10.*3.6e1+q1.*q2.*t2.*t3.*t5.*t8.*t11.*t17.*2.7e2-q1.*q2.*t3.*t4.*t5.*t8.*t9.*t17.*3.6e1-q3.*q4.*t2.*t3.*t5.*t7.*t8.*t17.*1.8e1-q3.*q4.*t2.*t5.*t6.*t8.*t9.*t12.*7.2e1+q3.*q4.*t4.*t5.*t6.*t8.*t9.*t10.*3.6e1-q1.*q2.*t2.*t3.*t5.*t8.*t12.*t17.*1.26e2-q1.*q2.*t3.*t4.*t5.*t8.*t10.*t17.*1.26e2+q1.*q2.*t2.*t5.*t7.*t8.*t9.*t17.*5.4e1-q3.*q4.*t2.*t3.*t5.*t8.*t9.*t17.*1.26e2+q3.*q4.*t3.*t5.*t6.*t8.*t10.*t12.*7.2e1+q1.*q2.*t2.*t5.*t7.*t8.*t10.*t17.*3.6e1+q1.*q2.*t5.*t6.*t8.*t9.*t10.*t11.*3.6e1+q3.*q4.*t2.*t3.*t5.*t8.*t10.*t17.*5.4e1+q3.*q4.*t2.*t4.*t5.*t8.*t9.*t17.*1.62e2+q3.*q4.*t5.*t6.*t7.*t8.*t9.*t10.*1.08e2+q1.*q2.*t3.*t5.*t7.*t8.*t10.*t17.*1.62e2-q1.*q2.*t5.*t6.*t8.*t9.*t10.*t12.*3.6e1-q3.*q4.*t2.*t3.*t5.*t8.*t11.*t17.*1.26e2+q3.*q4.*t3.*t4.*t5.*t8.*t9.*t17.*3.6e1-q1.*q2.*t2.*t5.*t8.*t9.*t10.*t17.*5.4e1+q3.*q4.*t2.*t3.*t5.*t8.*t12.*t17.*2.7e2+q3.*q4.*t3.*t4.*t5.*t8.*t10.*t17.*5.4e1+q1.*q2.*t2.*t5.*t8.*t9.*t11.*t17.*9.0e1+q1.*q2.*t3.*t5.*t8.*t9.*t10.*t17.*1.26e2-q3.*q4.*t2.*t5.*t7.*t8.*t9.*t17.*1.26e2-q1.*q2.*t2.*t5.*t8.*t9.*t12.*t17.*5.4e1+q1.*q2.*t2.*t5.*t8.*t10.*t11.*t17.*3.6e1-q1.*q2.*t4.*t5.*t8.*t9.*t10.*t17.*2.7e2-q3.*q4.*t2.*t5.*t7.*t8.*t10.*t17.*3.6e1-q3.*q4.*t5.*t6.*t8.*t9.*t10.*t11.*3.6e1-q1.*q2.*t3.*t5.*t8.*t9.*t12.*t17.*3.6e1+q1.*q2.*t3.*t5.*t8.*t10.*t11.*t17.*1.26e2-q3.*q4.*t3.*t5.*t7.*t8.*t10.*t17.*9.0e1+q3.*q4.*t5.*t6.*t8.*t9.*t10.*t12.*3.6e1-q1.*q2.*t3.*t5.*t8.*t10.*t12.*t17.*1.62e2+q3.*q4.*t2.*t5.*t8.*t9.*t10.*t17.*1.26e2+q1.*q2.*t5.*t7.*t8.*t9.*t10.*t17.*1.26e2-q3.*q4.*t2.*t5.*t8.*t9.*t11.*t17.*1.62e2-q3.*q4.*t3.*t5.*t8.*t9.*t10.*t17.*5.4e1+q3.*q4.*t2.*t5.*t8.*t9.*t12.*t17.*1.26e2-q3.*q4.*t2.*t5.*t8.*t10.*t11.*t17.*3.6e1+q3.*q4.*t4.*t5.*t8.*t9.*t10.*t17.*1.26e2+q3.*q4.*t3.*t5.*t8.*t9.*t12.*t17.*3.6e1-q3.*q4.*t3.*t5.*t8.*t10.*t11.*t17.*5.4e1+q3.*q4.*t3.*t5.*t8.*t10.*t12.*t17.*9.0e1+q1.*q2.*t5.*t8.*t9.*t10.*t11.*t17.*1.8e1-q3.*q4.*t5.*t7.*t8.*t9.*t10.*t17.*2.7e2-q1.*q2.*t5.*t8.*t9.*t10.*t12.*t17.*1.8e1-q3.*q4.*t5.*t8.*t9.*t10.*t11.*t17.*1.8e1+q3.*q4.*t5.*t8.*t9.*t10.*t12.*t17.*1.8e1-COM_Y.*COM_Z.*g.*rk.*t2.*t3.*t9.*t19.*t20.*4.8e1+COM_Y.*COM_Z.*g.*rk.*t2.*t3.*t10.*t19.*t20.*4.8e1-COM_Y.*COM_Z.*g.*rk.*t2.*t9.*t10.*t19.*t20.*4.8e1+COM_Y.*COM_Z.*g.*rk.*t3.*t9.*t10.*t19.*t20.*4.8e1-dx.*dy.*q1.*q3.*t2.*t3.*t4.*t5.*t8.*1.8e1+dx.*dy.*q2.*q4.*t2.*t3.*t4.*t5.*t8.*1.26e2+dx.*dy.*q1.*q3.*t2.*t3.*t5.*t7.*t8.*1.8e1+dx.*dy.*q1.*q3.*t2.*t3.*t5.*t8.*t9.*9.0e1-dx.*dy.*q2.*q4.*t2.*t3.*t5.*t7.*t8.*1.26e2-dx.*dy.*q1.*q3.*t2.*t3.*t5.*t8.*t10.*9.0e1-dx.*dy.*q1.*q3.*t2.*t4.*t5.*t8.*t9.*1.26e2+dx.*dy.*q1.*q3.*t2.*t3.*t5.*t8.*t11.*1.98e2-dx.*dy.*q1.*q3.*t3.*t4.*t5.*t8.*t9.*3.6e1-dx.*dy.*q2.*q4.*t2.*t3.*t5.*t8.*t9.*5.4e1-dx.*dy.*q1.*q3.*t2.*t3.*t5.*t8.*t12.*1.98e2-dx.*dy.*q1.*q3.*t3.*t4.*t5.*t8.*t10.*9.0e1+dx.*dy.*q2.*q4.*t2.*t3.*t5.*t8.*t10.*5.4e1+dx.*dy.*q2.*q4.*t2.*t4.*t5.*t8.*t9.*1.62e2+dx.*dy.*q1.*q3.*t2.*t5.*t7.*t8.*t9.*9.0e1+dx.*dy.*q2.*q4.*t2.*t3.*t5.*t8.*t11.*5.4e1-dx.*dy.*q2.*q4.*t3.*t4.*t5.*t8.*t9.*3.6e1+dx.*dy.*q1.*q3.*t2.*t5.*t7.*t8.*t10.*3.6e1-dx.*dy.*q2.*q4.*t2.*t3.*t5.*t8.*t12.*5.4e1-dx.*dy.*q2.*q4.*t3.*t4.*t5.*t8.*t10.*9.0e1+dx.*dy.*q1.*q3.*t3.*t5.*t7.*t8.*t10.*1.26e2+dx.*dy.*q2.*q4.*t2.*t5.*t7.*t8.*t9.*9.0e1+dx.*dy.*q1.*q3.*t2.*t5.*t8.*t9.*t10.*5.4e1+dx.*dy.*q2.*q4.*t2.*t5.*t7.*t8.*t10.*3.6e1-dx.*dy.*q1.*q3.*t2.*t5.*t8.*t9.*t11.*1.62e2-dx.*dy.*q1.*q3.*t3.*t5.*t8.*t9.*t10.*5.4e1-dx.*dy.*q2.*q4.*t3.*t5.*t7.*t8.*t10.*1.62e2-dx.*dy.*q1.*q3.*t2.*t5.*t8.*t9.*t12.*9.0e1+dx.*dy.*q1.*q3.*t2.*t5.*t8.*t10.*t11.*3.6e1-dx.*dy.*q1.*q3.*t4.*t5.*t8.*t9.*t10.*5.4e1-dx.*dy.*q2.*q4.*t2.*t5.*t8.*t9.*t10.*9.0e1-dx.*dy.*q1.*q3.*t3.*t5.*t8.*t9.*t12.*3.6e1+dx.*dy.*q1.*q3.*t3.*t5.*t8.*t10.*t11.*9.0e1+dx.*dy.*q2.*q4.*t2.*t5.*t8.*t9.*t11.*1.26e2+dx.*dy.*q2.*q4.*t3.*t5.*t8.*t9.*t10.*9.0e1+dx.*dy.*q1.*q3.*t3.*t5.*t8.*t10.*t12.*1.62e2-dx.*dy.*q2.*q4.*t2.*t5.*t8.*t9.*t12.*9.0e1+dx.*dy.*q2.*q4.*t2.*t5.*t8.*t10.*t11.*3.6e1-dx.*dy.*q2.*q4.*t4.*t5.*t8.*t9.*t10.*1.98e2+dx.*dy.*q1.*q3.*t5.*t7.*t8.*t9.*t10.*5.4e1-dx.*dy.*q2.*q4.*t3.*t5.*t8.*t9.*t12.*3.6e1+dx.*dy.*q2.*q4.*t3.*t5.*t8.*t10.*t11.*9.0e1-dx.*dy.*q2.*q4.*t3.*t5.*t8.*t10.*t12.*1.26e2+dx.*dy.*q2.*q4.*t5.*t7.*t8.*t9.*t10.*1.98e2-dx.*dy.*q1.*q3.*t5.*t8.*t9.*t10.*t11.*1.26e2+dx.*dy.*q1.*q3.*t5.*t8.*t9.*t10.*t12.*1.26e2+dx.*dy.*q2.*q4.*t5.*t8.*t9.*t10.*t11.*1.8e1-dx.*dy.*q2.*q4.*t5.*t8.*t9.*t10.*t12.*1.8e1-g.*q1.*q2.*rk.*t2.*t3.*t19.*t20.*t21.*3.2e1+g.*q1.*q2.*rk.*t2.*t3.*t19.*t20.*t23.*3.2e1-g.*q3.*q4.*rk.*t2.*t3.*t19.*t20.*t21.*3.2e1+g.*q3.*q4.*rk.*t2.*t3.*t19.*t20.*t23.*3.2e1+g.*q1.*q2.*rk.*t2.*t10.*t19.*t20.*t21.*3.2e1-g.*q1.*q2.*rk.*t3.*t9.*t19.*t20.*t21.*3.2e1-g.*q1.*q2.*rk.*t2.*t10.*t19.*t20.*t22.*3.2e1+g.*q1.*q2.*rk.*t3.*t9.*t19.*t20.*t22.*3.2e1-g.*q3.*q4.*rk.*t2.*t10.*t19.*t20.*t21.*3.2e1+g.*q3.*q4.*rk.*t3.*t9.*t19.*t20.*t21.*3.2e1+g.*q3.*q4.*rk.*t2.*t10.*t19.*t20.*t22.*3.2e1-g.*q3.*q4.*rk.*t3.*t9.*t19.*t20.*t22.*3.2e1+g.*q1.*q2.*rk.*t9.*t10.*t19.*t20.*t21.*3.2e1-g.*q1.*q2.*rk.*t9.*t10.*t19.*t20.*t23.*3.2e1+g.*q3.*q4.*rk.*t9.*t10.*t19.*t20.*t21.*3.2e1-g.*q3.*q4.*rk.*t9.*t10.*t19.*t20.*t23.*3.2e1-COM_X.*Jw.*Mb.*g.*q1.*q4.*t4.*t8.*t18.*1.2e1+COM_X.*Jw.*Mb.*g.*q2.*q3.*t4.*t8.*t18.*1.2e1-COM_Z.*Jw.*Mb.*g.*q1.*q2.*t4.*t8.*t18.*1.2e1+COM_X.*Jw.*Mb.*g.*q1.*q4.*t7.*t8.*t18.*1.2e1-COM_X.*Jw.*Mb.*g.*q2.*q3.*t7.*t8.*t18.*1.2e1-COM_Z.*Jw.*Mb.*g.*q1.*q2.*t7.*t8.*t18.*1.2e1+COM_Z.*Jw.*Mb.*g.*q3.*q4.*t4.*t8.*t18.*1.2e1+COM_X.*Jw.*Mb.*g.*q1.*q4.*t8.*t11.*t18.*1.2e1-COM_X.*Jw.*Mb.*g.*q2.*q3.*t8.*t11.*t18.*1.2e1-COM_Z.*Jw.*Mb.*g.*q1.*q2.*t8.*t11.*t18.*1.2e1+COM_Z.*Jw.*Mb.*g.*q3.*q4.*t7.*t8.*t18.*1.2e1-COM_X.*Jw.*Mb.*g.*q1.*q4.*t8.*t12.*t18.*1.2e1+COM_X.*Jw.*Mb.*g.*q2.*q3.*t8.*t12.*t18.*1.2e1-COM_Z.*Jw.*Mb.*g.*q1.*q2.*t8.*t12.*t18.*1.2e1+COM_Z.*Jw.*Mb.*g.*q3.*q4.*t8.*t11.*t18.*1.2e1+COM_Z.*Jw.*Mb.*g.*q3.*q4.*t8.*t12.*t18.*1.2e1-COM_Y.*Jw.*Mb.*g.*t2.*t3.*t4.*t8.*t18.*2.4e1-COM_Y.*Jw.*Mb.*g.*t2.*t3.*t7.*t8.*t18.*2.4e1-COM_Y.*Jw.*Mb.*g.*t2.*t3.*t8.*t9.*t18.*1.2e1-COM_Y.*Jw.*Mb.*g.*t2.*t3.*t8.*t10.*t18.*1.2e1-COM_Y.*Jw.*Mb.*g.*t2.*t4.*t8.*t9.*t18.*1.2e1+COM_Y.*Jw.*Mb.*g.*t2.*t3.*t8.*t11.*t18.*4.8e1-COM_Y.*Jw.*Mb.*g.*t2.*t4.*t8.*t10.*t18.*3.6e1-COM_Y.*Jw.*Mb.*g.*t3.*t4.*t8.*t9.*t18.*1.2e1+COM_Y.*Jw.*Mb.*g.*t2.*t3.*t8.*t12.*t18.*4.8e1-COM_Y.*Jw.*Mb.*g.*t3.*t4.*t8.*t10.*t18.*3.6e1-COM_Y.*Jw.*Mb.*g.*t2.*t7.*t8.*t9.*t18.*3.6e1-COM_Y.*Jw.*Mb.*g.*t2.*t7.*t8.*t10.*t18.*1.2e1-COM_Y.*Jw.*Mb.*g.*t3.*t7.*t8.*t9.*t18.*3.6e1-COM_Y.*Jw.*Mb.*g.*t3.*t7.*t8.*t10.*t18.*1.2e1+COM_Y.*Jw.*Mb.*g.*t2.*t8.*t9.*t10.*t18.*1.2e1+COM_Y.*Jw.*Mb.*g.*t2.*t8.*t9.*t11.*t18.*1.2e1+COM_Y.*Jw.*Mb.*g.*t3.*t8.*t9.*t10.*t18.*1.2e1+COM_Y.*Jw.*Mb.*g.*t2.*t8.*t9.*t12.*t18.*3.6e1+COM_Y.*Jw.*Mb.*g.*t2.*t8.*t10.*t11.*t18.*1.2e1+COM_Y.*Jw.*Mb.*g.*t3.*t8.*t9.*t11.*t18.*3.6e1-COM_Y.*Jw.*Mb.*g.*t4.*t8.*t9.*t10.*t18.*4.8e1+COM_Y.*Jw.*Mb.*g.*t2.*t8.*t10.*t12.*t18.*3.6e1+COM_Y.*Jw.*Mb.*g.*t3.*t8.*t9.*t12.*t18.*1.2e1+COM_Y.*Jw.*Mb.*g.*t3.*t8.*t10.*t11.*t18.*3.6e1+COM_Y.*Jw.*Mb.*g.*t3.*t8.*t10.*t12.*t18.*1.2e1-COM_Y.*Jw.*Mb.*g.*t7.*t8.*t9.*t10.*t18.*4.8e1+COM_Y.*Jw.*Mb.*g.*t8.*t9.*t10.*t11.*t18.*2.4e1+COM_Y.*Jw.*Mb.*g.*t8.*t9.*t10.*t12.*t18.*2.4e1+COM_Y.*Jw.*Mb.*rk.*t2.*t4.*t9.*t17.*t18.*2.4e1+COM_Y.*Jw.*Mb.*rk.*t2.*t3.*t11.*t17.*t18.*4.8e1+COM_Y.*Jw.*Mb.*rk.*t2.*t3.*t12.*t17.*t18.*4.8e1-COM_Y.*Jw.*Mb.*rk.*t3.*t4.*t10.*t17.*t18.*2.4e1-COM_Y.*Jw.*Mb.*rk.*t2.*t7.*t9.*t17.*t18.*2.4e1+COM_Y.*Jw.*Mb.*rk.*t3.*t7.*t10.*t17.*t18.*2.4e1-COM_Y.*Jw.*Mb.*rk.*t2.*t9.*t11.*t17.*t18.*2.4e1+COM_Y.*Jw.*Mb.*rk.*t2.*t9.*t12.*t17.*t18.*2.4e1-COM_Y.*Jw.*Mb.*rk.*t4.*t9.*t10.*t17.*t18.*4.8e1+COM_Y.*Jw.*Mb.*rk.*t3.*t10.*t11.*t17.*t18.*2.4e1-COM_Y.*Jw.*Mb.*rk.*t3.*t10.*t12.*t17.*t18.*2.4e1-COM_Y.*Jw.*Mb.*rk.*t7.*t9.*t10.*t17.*t18.*4.8e1-COM_X.*COM_Y.*g.*q1.*q3.*rk.*t4.*t19.*t20.*1.6e1+COM_X.*COM_Y.*g.*q2.*q4.*rk.*t4.*t19.*t20.*4.8e1-COM_X.*COM_Z.*g.*q1.*q4.*rk.*t4.*t19.*t20.*4.8e1-COM_X.*COM_Z.*g.*q2.*q3.*rk.*t4.*t19.*t20.*1.6e1-COM_X.*COM_Y.*g.*q1.*q3.*rk.*t7.*t19.*t20.*1.6e1+COM_X.*COM_Y.*g.*q2.*q4.*rk.*t7.*t19.*t20.*4.8e1+COM_X.*COM_Z.*g.*q1.*q4.*rk.*t7.*t19.*t20.*1.6e1+COM_X.*COM_Z.*g.*q2.*q3.*rk.*t7.*t19.*t20.*4.8e1+COM_X.*COM_Y.*g.*q1.*q3.*rk.*t11.*t19.*t20.*4.8e1+COM_X.*COM_Y.*g.*q1.*q3.*rk.*t12.*t19.*t20.*4.8e1-COM_X.*COM_Y.*g.*q2.*q4.*rk.*t11.*t19.*t20.*1.6e1+COM_X.*COM_Z.*g.*q1.*q4.*rk.*t11.*t19.*t20.*1.6e1+COM_X.*COM_Z.*g.*q2.*q3.*rk.*t11.*t19.*t20.*4.8e1-COM_X.*COM_Y.*g.*q2.*q4.*rk.*t12.*t19.*t20.*1.6e1-COM_X.*COM_Z.*g.*q1.*q4.*rk.*t12.*t19.*t20.*4.8e1-COM_X.*COM_Z.*g.*q2.*q3.*rk.*t12.*t19.*t20.*1.6e1-Jw.*Mb.*dx.*dy.*q1.*q3.*t2.*t8.*t18.*1.2e1+Jw.*Mb.*dx.*dy.*q1.*q3.*t3.*t8.*t18.*1.2e1-Jw.*Mb.*dx.*dy.*q2.*q4.*t2.*t8.*t18.*1.2e1+Jw.*Mb.*dx.*dy.*q2.*q4.*t3.*t8.*t18.*1.2e1+Jw.*Mb.*dx.*dy.*q1.*q3.*t8.*t9.*t18.*1.2e1-Jw.*Mb.*dx.*dy.*q1.*q3.*t8.*t10.*t18.*1.2e1+Jw.*Mb.*dx.*dy.*q2.*q4.*t8.*t9.*t18.*1.2e1-Jw.*Mb.*dx.*dy.*q2.*q4.*t8.*t10.*t18.*1.2e1-Jw.*Mk.*dx.*dy.*q1.*q3.*t2.*t8.*t18.*1.2e1+Jw.*Mk.*dx.*dy.*q1.*q3.*t3.*t8.*t18.*1.2e1-Jw.*Mk.*dx.*dy.*q2.*q4.*t2.*t8.*t18.*1.2e1+Jw.*Mk.*dx.*dy.*q2.*q4.*t3.*t8.*t18.*1.2e1+Jw.*Mk.*dx.*dy.*q1.*q3.*t8.*t9.*t18.*1.2e1-Jw.*Mk.*dx.*dy.*q1.*q3.*t8.*t10.*t18.*1.2e1+Jw.*Mk.*dx.*dy.*q2.*q4.*t8.*t9.*t18.*1.2e1-Jw.*Mk.*dx.*dy.*q2.*q4.*t8.*t10.*t18.*1.2e1-COM_X.*Jw.*Mb.*dx.*dy.*rk.*t2.*t3.*t4.*t18.*2.4e1+COM_X.*Jw.*Mb.*dx.*dy.*rk.*t2.*t3.*t7.*t18.*2.4e1-COM_X.*Jw.*Mb.*dx.*dy.*rk.*t2.*t4.*t9.*t18.*4.8e1+COM_X.*Jw.*Mb.*dx.*dy.*rk.*t2.*t3.*t11.*t18.*2.4e1-COM_X.*Jw.*Mb.*dx.*dy.*rk.*t2.*t3.*t12.*t18.*2.4e1+COM_X.*Jw.*Mb.*dx.*dy.*rk.*t3.*t7.*t10.*t18.*4.8e1-COM_X.*Jw.*Mb.*dx.*dy.*rk.*t2.*t9.*t11.*t18.*4.8e1+COM_X.*Jw.*Mb.*dx.*dy.*rk.*t4.*t9.*t10.*t18.*2.4e1+COM_X.*Jw.*Mb.*dx.*dy.*rk.*t3.*t10.*t12.*t18.*4.8e1-COM_X.*Jw.*Mb.*dx.*dy.*rk.*t7.*t9.*t10.*t18.*2.4e1-COM_X.*Jw.*Mb.*dx.*dy.*rk.*t9.*t10.*t11.*t18.*2.4e1+COM_X.*Jw.*Mb.*dx.*dy.*rk.*t9.*t10.*t12.*t18.*2.4e1+COM_X.*Jw.*Mb.*g.*q1.*q4.*t2.*t3.*t8.*t18.*4.8e1+COM_X.*Jw.*Mb.*g.*q2.*q3.*t2.*t3.*t8.*t18.*4.8e1-COM_Z.*Jw.*Mb.*g.*q1.*q2.*t2.*t3.*t8.*t18.*7.2e1-COM_X.*Jw.*Mb.*g.*q1.*q4.*t2.*t4.*t8.*t18.*3.6e1-COM_X.*Jw.*Mb.*g.*q2.*q3.*t2.*t4.*t8.*t18.*6.0e1+COM_Z.*Jw.*Mb.*g.*q1.*q2.*t2.*t4.*t8.*t18.*8.4e1+COM_X.*Jw.*Mb.*g.*q1.*q4.*t3.*t4.*t8.*t18.*1.2e1-COM_X.*Jw.*Mb.*g.*q2.*q3.*t3.*t4.*t8.*t18.*1.2e1+COM_Z.*Jw.*Mb.*g.*q1.*q2.*t3.*t4.*t8.*t18.*1.2e1-COM_X.*Jw.*Mb.*g.*q1.*q4.*t2.*t7.*t8.*t18.*1.2e1+COM_X.*Jw.*Mb.*g.*q2.*q3.*t2.*t7.*t8.*t18.*1.2e1+COM_Z.*Jw.*Mb.*g.*q1.*q2.*t2.*t7.*t8.*t18.*1.2e1+COM_Z.*Jw.*Mb.*g.*q3.*q4.*t2.*t3.*t8.*t18.*7.2e1-COM_X.*Jw.*Mb.*g.*q1.*q4.*t3.*t7.*t8.*t18.*6.0e1-COM_X.*Jw.*Mb.*g.*q2.*q3.*t3.*t7.*t8.*t18.*3.6e1+COM_Z.*Jw.*Mb.*g.*q1.*q2.*t3.*t7.*t8.*t18.*8.4e1-COM_Z.*Jw.*Mb.*g.*q3.*q4.*t2.*t4.*t8.*t18.*8.4e1+COM_X.*Jw.*Mb.*g.*q1.*q4.*t2.*t8.*t9.*t18.*4.8e1+COM_X.*Jw.*Mb.*g.*q2.*q3.*t2.*t8.*t9.*t18.*4.8e1-COM_Z.*Jw.*Mb.*g.*q1.*q2.*t2.*t8.*t9.*t18.*7.2e1-COM_Z.*Jw.*Mb.*g.*q3.*q4.*t3.*t4.*t8.*t18.*1.2e1+COM_X.*Jw.*Mb.*g.*q1.*q4.*t2.*t8.*t10.*t18.*2.4e1-COM_X.*Jw.*Mb.*g.*q1.*q4.*t3.*t8.*t9.*t18.*2.4e1-COM_X.*Jw.*Mb.*g.*q2.*q3.*t2.*t8.*t10.*t18.*2.4e1+COM_X.*Jw.*Mb.*g.*q2.*q3.*t3.*t8.*t9.*t18.*2.4e1-COM_Z.*Jw.*Mb.*g.*q1.*q2.*t2.*t8.*t10.*t18.*2.4e1-COM_Z.*Jw.*Mb.*g.*q1.*q2.*t3.*t8.*t9.*t18.*2.4e1-COM_X.*Jw.*Mb.*g.*q1.*q4.*t2.*t8.*t11.*t18.*3.6e1+COM_X.*Jw.*Mb.*g.*q1.*q4.*t3.*t8.*t10.*t18.*4.8e1+COM_X.*Jw.*Mb.*g.*q1.*q4.*t4.*t8.*t9.*t18.*3.6e1+COM_X.*Jw.*Mb.*g.*q2.*q3.*t2.*t8.*t11.*t18.*3.6e1+COM_X.*Jw.*Mb.*g.*q2.*q3.*t3.*t8.*t10.*t18.*4.8e1-COM_X.*Jw.*Mb.*g.*q2.*q3.*t4.*t8.*t9.*t18.*3.6e1+COM_Z.*Jw.*Mb.*g.*q1.*q2.*t2.*t8.*t11.*t18.*3.6e1-COM_Z.*Jw.*Mb.*g.*q1.*q2.*t3.*t8.*t10.*t18.*7.2e1+COM_Z.*Jw.*Mb.*g.*q1.*q2.*t4.*t8.*t9.*t18.*3.6e1-COM_Z.*Jw.*Mb.*g.*q3.*q4.*t2.*t7.*t8.*t18.*1.2e1-COM_X.*Jw.*Mb.*g.*q1.*q4.*t2.*t8.*t12.*t18.*3.6e1-COM_X.*Jw.*Mb.*g.*q1.*q4.*t3.*t8.*t11.*t18.*6.0e1-COM_X.*Jw.*Mb.*g.*q1.*q4.*t4.*t8.*t10.*t18.*3.6e1-COM_X.*Jw.*Mb.*g.*q2.*q3.*t2.*t8.*t12.*t18.*6.0e1-COM_X.*Jw.*Mb.*g.*q2.*q3.*t3.*t8.*t11.*t18.*3.6e1-COM_X.*Jw.*Mb.*g.*q2.*q3.*t4.*t8.*t10.*t18.*6.0e1+COM_Z.*Jw.*Mb.*g.*q1.*q2.*t2.*t8.*t12.*t18.*8.4e1+COM_Z.*Jw.*Mb.*g.*q1.*q2.*t3.*t8.*t11.*t18.*8.4e1+COM_Z.*Jw.*Mb.*g.*q1.*q2.*t4.*t8.*t10.*t18.*8.4e1-COM_Z.*Jw.*Mb.*g.*q3.*q4.*t3.*t7.*t8.*t18.*8.4e1+COM_X.*Jw.*Mb.*g.*q1.*q4.*t3.*t8.*t12.*t18.*3.6e1-COM_X.*Jw.*Mb.*g.*q2.*q3.*t3.*t8.*t12.*t18.*3.6e1+COM_Z.*Jw.*Mb.*g.*q1.*q2.*t3.*t8.*t12.*t18.*3.6e1+COM_Z.*Jw.*Mb.*g.*q3.*q4.*t2.*t8.*t9.*t18.*7.2e1-COM_X.*Jw.*Mb.*g.*q1.*q4.*t7.*t8.*t9.*t18.*6.0e1-COM_X.*Jw.*Mb.*g.*q2.*q3.*t7.*t8.*t9.*t18.*3.6e1+COM_Z.*Jw.*Mb.*g.*q1.*q2.*t7.*t8.*t9.*t18.*8.4e1+COM_Z.*Jw.*Mb.*g.*q3.*q4.*t2.*t8.*t10.*t18.*2.4e1+COM_Z.*Jw.*Mb.*g.*q3.*q4.*t3.*t8.*t9.*t18.*2.4e1-COM_X.*Jw.*Mb.*g.*q1.*q4.*t7.*t8.*t10.*t18.*3.6e1+COM_X.*Jw.*Mb.*g.*q2.*q3.*t7.*t8.*t10.*t18.*3.6e1+COM_Z.*Jw.*Mb.*g.*q1.*q2.*t7.*t8.*t10.*t18.*3.6e1-COM_Z.*Jw.*Mb.*g.*q3.*q4.*t2.*t8.*t11.*t18.*3.6e1+COM_Z.*Jw.*Mb.*g.*q3.*q4.*t3.*t8.*t10.*t18.*7.2e1-COM_Z.*Jw.*Mb.*g.*q3.*q4.*t4.*t8.*t9.*t18.*3.6e1-COM_Z.*Jw.*Mb.*g.*q3.*q4.*t2.*t8.*t12.*t18.*8.4e1-COM_Z.*Jw.*Mb.*g.*q3.*q4.*t3.*t8.*t11.*t18.*8.4e1-COM_Z.*Jw.*Mb.*g.*q3.*q4.*t4.*t8.*t10.*t18.*8.4e1+COM_X.*Jw.*Mb.*g.*q1.*q4.*t8.*t9.*t10.*t18.*4.8e1+COM_X.*Jw.*Mb.*g.*q2.*q3.*t8.*t9.*t10.*t18.*4.8e1-COM_Z.*Jw.*Mb.*g.*q1.*q2.*t8.*t9.*t10.*t18.*7.2e1-COM_Z.*Jw.*Mb.*g.*q3.*q4.*t3.*t8.*t12.*t18.*3.6e1-COM_X.*Jw.*Mb.*g.*q1.*q4.*t8.*t9.*t11.*t18.*6.0e1-COM_X.*Jw.*Mb.*g.*q2.*q3.*t8.*t9.*t11.*t18.*3.6e1+COM_Z.*Jw.*Mb.*g.*q1.*q2.*t8.*t9.*t11.*t18.*8.4e1-COM_Z.*Jw.*Mb.*g.*q3.*q4.*t7.*t8.*t9.*t18.*8.4e1+COM_X.*Jw.*Mb.*g.*q1.*q4.*t8.*t9.*t12.*t18.*1.2e1-COM_X.*Jw.*Mb.*g.*q1.*q4.*t8.*t10.*t11.*t18.*1.2e1-COM_X.*Jw.*Mb.*g.*q2.*q3.*t8.*t9.*t12.*t18.*1.2e1+COM_X.*Jw.*Mb.*g.*q2.*q3.*t8.*t10.*t11.*t18.*1.2e1+COM_Z.*Jw.*Mb.*g.*q1.*q2.*t8.*t9.*t12.*t18.*1.2e1+COM_Z.*Jw.*Mb.*g.*q1.*q2.*t8.*t10.*t11.*t18.*1.2e1-COM_Z.*Jw.*Mb.*g.*q3.*q4.*t7.*t8.*t10.*t18.*3.6e1-COM_X.*Jw.*Mb.*g.*q1.*q4.*t8.*t10.*t12.*t18.*3.6e1-COM_X.*Jw.*Mb.*g.*q2.*q3.*t8.*t10.*t12.*t18.*6.0e1+COM_Z.*Jw.*Mb.*g.*q1.*q2.*t8.*t10.*t12.*t18.*8.4e1+COM_Z.*Jw.*Mb.*g.*q3.*q4.*t8.*t9.*t10.*t18.*7.2e1-COM_Z.*Jw.*Mb.*g.*q3.*q4.*t8.*t9.*t11.*t18.*8.4e1-COM_Z.*Jw.*Mb.*g.*q3.*q4.*t8.*t9.*t12.*t18.*1.2e1-COM_Z.*Jw.*Mb.*g.*q3.*q4.*t8.*t10.*t11.*t18.*1.2e1-COM_Z.*Jw.*Mb.*g.*q3.*q4.*t8.*t10.*t12.*t18.*8.4e1-COM_X.*Jw.*Mb.*q2.*q3.*rk.*t2.*t4.*t17.*t18.*2.4e1+COM_Z.*Jw.*Mb.*q1.*q2.*rk.*t2.*t4.*t17.*t18.*1.2e1+COM_X.*Jw.*Mb.*q1.*q4.*rk.*t3.*t4.*t17.*t18.*2.4e1-COM_Z.*Jw.*Mb.*q1.*q2.*rk.*t3.*t4.*t17.*t18.*1.2e1+COM_X.*Jw.*Mb.*q2.*q3.*rk.*t2.*t7.*t17.*t18.*2.4e1-COM_Z.*Jw.*Mb.*q1.*q2.*rk.*t2.*t7.*t17.*t18.*1.2e1-COM_X.*Jw.*Mb.*q1.*q4.*rk.*t3.*t7.*t17.*t18.*2.4e1+COM_Z.*Jw.*Mb.*q1.*q2.*rk.*t3.*t7.*t17.*t18.*1.2e1-COM_Z.*Jw.*Mb.*q3.*q4.*rk.*t2.*t4.*t17.*t18.*1.2e1+COM_Z.*Jw.*Mb.*q3.*q4.*rk.*t3.*t4.*t17.*t18.*1.2e1+COM_X.*Jw.*Mb.*q1.*q4.*rk.*t4.*t9.*t17.*t18.*7.2e1+COM_X.*Jw.*Mb.*q2.*q3.*rk.*t2.*t11.*t17.*t18.*7.2e1-COM_Z.*Jw.*Mb.*q1.*q2.*rk.*t2.*t11.*t17.*t18.*3.6e1-COM_Z.*Jw.*Mb.*q1.*q2.*rk.*t4.*t9.*t17.*t18.*3.6e1+COM_Z.*Jw.*Mb.*q3.*q4.*rk.*t2.*t7.*t17.*t18.*1.2e1-COM_X.*Jw.*Mb.*q1.*q4.*rk.*t3.*t11.*t17.*t18.*2.4e1-COM_X.*Jw.*Mb.*q2.*q3.*rk.*t2.*t12.*t17.*t18.*2.4e1-COM_X.*Jw.*Mb.*q2.*q3.*rk.*t4.*t10.*t17.*t18.*2.4e1+COM_Z.*Jw.*Mb.*q1.*q2.*rk.*t2.*t12.*t17.*t18.*1.2e1+COM_Z.*Jw.*Mb.*q1.*q2.*rk.*t3.*t11.*t17.*t18.*1.2e1+COM_Z.*Jw.*Mb.*q1.*q2.*rk.*t4.*t10.*t17.*t18.*1.2e1-COM_Z.*Jw.*Mb.*q3.*q4.*rk.*t3.*t7.*t17.*t18.*1.2e1+COM_X.*Jw.*Mb.*q1.*q4.*rk.*t3.*t12.*t17.*t18.*7.2e1-COM_Z.*Jw.*Mb.*q1.*q2.*rk.*t3.*t12.*t17.*t18.*3.6e1-COM_X.*Jw.*Mb.*q1.*q4.*rk.*t7.*t9.*t17.*t18.*2.4e1+COM_Z.*Jw.*Mb.*q1.*q2.*rk.*t7.*t9.*t17.*t18.*1.2e1+COM_X.*Jw.*Mb.*q2.*q3.*rk.*t7.*t10.*t17.*t18.*7.2e1-COM_Z.*Jw.*Mb.*q1.*q2.*rk.*t7.*t10.*t17.*t18.*3.6e1+COM_Z.*Jw.*Mb.*q3.*q4.*rk.*t2.*t11.*t17.*t18.*3.6e1+COM_Z.*Jw.*Mb.*q3.*q4.*rk.*t4.*t9.*t17.*t18.*3.6e1-COM_Z.*Jw.*Mb.*q3.*q4.*rk.*t2.*t12.*t17.*t18.*1.2e1-COM_Z.*Jw.*Mb.*q3.*q4.*rk.*t3.*t11.*t17.*t18.*1.2e1-COM_Z.*Jw.*Mb.*q3.*q4.*rk.*t4.*t10.*t17.*t18.*1.2e1+COM_Z.*Jw.*Mb.*q3.*q4.*rk.*t3.*t12.*t17.*t18.*3.6e1-COM_X.*Jw.*Mb.*q1.*q4.*rk.*t9.*t11.*t17.*t18.*2.4e1+COM_Z.*Jw.*Mb.*q1.*q2.*rk.*t9.*t11.*t17.*t18.*1.2e1-COM_Z.*Jw.*Mb.*q3.*q4.*rk.*t7.*t9.*t17.*t18.*1.2e1+COM_X.*Jw.*Mb.*q1.*q4.*rk.*t9.*t12.*t17.*t18.*2.4e1+COM_X.*Jw.*Mb.*q2.*q3.*rk.*t10.*t11.*t17.*t18.*2.4e1-COM_Z.*Jw.*Mb.*q1.*q2.*rk.*t9.*t12.*t17.*t18.*1.2e1-COM_Z.*Jw.*Mb.*q1.*q2.*rk.*t10.*t11.*t17.*t18.*1.2e1+COM_Z.*Jw.*Mb.*q3.*q4.*rk.*t7.*t10.*t17.*t18.*3.6e1-COM_X.*Jw.*Mb.*q2.*q3.*rk.*t10.*t12.*t17.*t18.*2.4e1+COM_Z.*Jw.*Mb.*q1.*q2.*rk.*t10.*t12.*t17.*t18.*1.2e1-COM_Z.*Jw.*Mb.*q3.*q4.*rk.*t9.*t11.*t17.*t18.*1.2e1+COM_Z.*Jw.*Mb.*q3.*q4.*rk.*t9.*t12.*t17.*t18.*1.2e1+COM_Z.*Jw.*Mb.*q3.*q4.*rk.*t10.*t11.*t17.*t18.*1.2e1-COM_Z.*Jw.*Mb.*q3.*q4.*rk.*t10.*t12.*t17.*t18.*1.2e1-COM_X.*COM_Y.*g.*q1.*q3.*rk.*t2.*t3.*t19.*t20.*3.2e1+COM_X.*COM_Y.*g.*q2.*q4.*rk.*t2.*t3.*t19.*t20.*9.6e1-COM_X.*COM_Z.*g.*q1.*q4.*rk.*t2.*t3.*t19.*t20.*3.2e1+COM_X.*COM_Z.*g.*q2.*q3.*rk.*t2.*t3.*t19.*t20.*3.2e1+COM_X.*COM_Y.*g.*q1.*q3.*rk.*t2.*t9.*t19.*t20.*3.2e1+COM_X.*COM_Y.*g.*q1.*q3.*rk.*t2.*t10.*t19.*t20.*3.2e1+COM_X.*COM_Y.*g.*q1.*q3.*rk.*t3.*t9.*t19.*t20.*3.2e1+COM_X.*COM_Y.*g.*q1.*q3.*rk.*t3.*t10.*t19.*t20.*3.2e1+COM_X.*COM_Y.*g.*q2.*q4.*rk.*t2.*t9.*t19.*t20.*3.2e1-COM_X.*COM_Z.*g.*q1.*q4.*rk.*t2.*t9.*t19.*t20.*3.2e1+COM_X.*COM_Z.*g.*q2.*q3.*rk.*t2.*t9.*t19.*t20.*3.2e1+COM_X.*COM_Y.*g.*q2.*q4.*rk.*t2.*t10.*t19.*t20.*3.2e1+COM_X.*COM_Y.*g.*q2.*q4.*rk.*t3.*t9.*t19.*t20.*3.2e1+COM_X.*COM_Z.*g.*q1.*q4.*rk.*t2.*t10.*t19.*t20.*3.2e1-COM_X.*COM_Z.*g.*q1.*q4.*rk.*t3.*t9.*t19.*t20.*9.6e1+COM_X.*COM_Z.*g.*q2.*q3.*rk.*t2.*t10.*t19.*t20.*9.6e1-COM_X.*COM_Z.*g.*q2.*q3.*rk.*t3.*t9.*t19.*t20.*3.2e1+COM_X.*COM_Y.*g.*q2.*q4.*rk.*t3.*t10.*t19.*t20.*3.2e1-COM_X.*COM_Z.*g.*q1.*q4.*rk.*t3.*t10.*t19.*t20.*3.2e1+COM_X.*COM_Z.*g.*q2.*q3.*rk.*t3.*t10.*t19.*t20.*3.2e1+COM_X.*COM_Y.*g.*q1.*q3.*rk.*t9.*t10.*t19.*t20.*9.6e1-COM_X.*COM_Y.*g.*q2.*q4.*rk.*t9.*t10.*t19.*t20.*3.2e1-COM_X.*COM_Z.*g.*q1.*q4.*rk.*t9.*t10.*t19.*t20.*3.2e1+COM_X.*COM_Z.*g.*q2.*q3.*rk.*t9.*t10.*t19.*t20.*3.2e1+COM_Y.*Jw.*Mb.*dx.*dy.*q2.*q3.*rk.*t2.*t4.*t18.*2.4e1+COM_Z.*Jw.*Mb.*dx.*dy.*q1.*q3.*rk.*t2.*t4.*t18.*1.2e1+COM_Y.*Jw.*Mb.*dx.*dy.*q1.*q4.*rk.*t3.*t4.*t18.*2.4e1-COM_Z.*Jw.*Mb.*dx.*dy.*q1.*q3.*rk.*t3.*t4.*t18.*1.2e1+COM_Z.*Jw.*Mb.*dx.*dy.*q2.*q4.*rk.*t2.*t4.*t18.*1.2e1-COM_Y.*Jw.*Mb.*dx.*dy.*q2.*q3.*rk.*t2.*t7.*t18.*2.4e1-COM_Z.*Jw.*Mb.*dx.*dy.*q1.*q3.*rk.*t2.*t7.*t18.*1.2e1-COM_Z.*Jw.*Mb.*dx.*dy.*q2.*q4.*rk.*t3.*t4.*t18.*1.2e1-COM_Y.*Jw.*Mb.*dx.*dy.*q1.*q4.*rk.*t3.*t7.*t18.*2.4e1+COM_Z.*Jw.*Mb.*dx.*dy.*q1.*q3.*rk.*t3.*t7.*t18.*1.2e1-COM_Z.*Jw.*Mb.*dx.*dy.*q2.*q4.*rk.*t2.*t7.*t18.*1.2e1+COM_Z.*Jw.*Mb.*dx.*dy.*q2.*q4.*rk.*t3.*t7.*t18.*1.2e1+COM_Y.*Jw.*Mb.*dx.*dy.*q1.*q4.*rk.*t4.*t9.*t18.*7.2e1-COM_Y.*Jw.*Mb.*dx.*dy.*q2.*q3.*rk.*t2.*t11.*t18.*7.2e1-COM_Z.*Jw.*Mb.*dx.*dy.*q1.*q3.*rk.*t2.*t11.*t18.*3.6e1-COM_Z.*Jw.*Mb.*dx.*dy.*q1.*q3.*rk.*t4.*t9.*t18.*3.6e1-COM_Y.*Jw.*Mb.*dx.*dy.*q1.*q4.*rk.*t3.*t11.*t18.*2.4e1+COM_Y.*Jw.*Mb.*dx.*dy.*q2.*q3.*rk.*t2.*t12.*t18.*2.4e1+COM_Y.*Jw.*Mb.*dx.*dy.*q2.*q3.*rk.*t4.*t10.*t18.*2.4e1+COM_Z.*Jw.*Mb.*dx.*dy.*q1.*q3.*rk.*t2.*t12.*t18.*1.2e1+COM_Z.*Jw.*Mb.*dx.*dy.*q1.*q3.*rk.*t3.*t11.*t18.*1.2e1+COM_Z.*Jw.*Mb.*dx.*dy.*q1.*q3.*rk.*t4.*t10.*t18.*1.2e1+COM_Y.*Jw.*Mb.*dx.*dy.*q1.*q4.*rk.*t3.*t12.*t18.*7.2e1-COM_Z.*Jw.*Mb.*dx.*dy.*q1.*q3.*rk.*t3.*t12.*t18.*3.6e1-COM_Z.*Jw.*Mb.*dx.*dy.*q2.*q4.*rk.*t2.*t11.*t18.*3.6e1-COM_Z.*Jw.*Mb.*dx.*dy.*q2.*q4.*rk.*t4.*t9.*t18.*3.6e1-COM_Y.*Jw.*Mb.*dx.*dy.*q1.*q4.*rk.*t7.*t9.*t18.*2.4e1+COM_Z.*Jw.*Mb.*dx.*dy.*q1.*q3.*rk.*t7.*t9.*t18.*1.2e1+COM_Z.*Jw.*Mb.*dx.*dy.*q2.*q4.*rk.*t2.*t12.*t18.*1.2e1+COM_Z.*Jw.*Mb.*dx.*dy.*q2.*q4.*rk.*t3.*t11.*t18.*1.2e1+COM_Z.*Jw.*Mb.*dx.*dy.*q2.*q4.*rk.*t4.*t10.*t18.*1.2e1-COM_Y.*Jw.*Mb.*dx.*dy.*q2.*q3.*rk.*t7.*t10.*t18.*7.2e1-COM_Z.*Jw.*Mb.*dx.*dy.*q1.*q3.*rk.*t7.*t10.*t18.*3.6e1-COM_Z.*Jw.*Mb.*dx.*dy.*q2.*q4.*rk.*t3.*t12.*t18.*3.6e1+COM_Z.*Jw.*Mb.*dx.*dy.*q2.*q4.*rk.*t7.*t9.*t18.*1.2e1-COM_Z.*Jw.*Mb.*dx.*dy.*q2.*q4.*rk.*t7.*t10.*t18.*3.6e1-COM_Y.*Jw.*Mb.*dx.*dy.*q1.*q4.*rk.*t9.*t11.*t18.*2.4e1+COM_Z.*Jw.*Mb.*dx.*dy.*q1.*q3.*rk.*t9.*t11.*t18.*1.2e1+COM_Y.*Jw.*Mb.*dx.*dy.*q1.*q4.*rk.*t9.*t12.*t18.*2.4e1-COM_Y.*Jw.*Mb.*dx.*dy.*q2.*q3.*rk.*t10.*t11.*t18.*2.4e1-COM_Z.*Jw.*Mb.*dx.*dy.*q1.*q3.*rk.*t9.*t12.*t18.*1.2e1-COM_Z.*Jw.*Mb.*dx.*dy.*q1.*q3.*rk.*t10.*t11.*t18.*1.2e1+COM_Y.*Jw.*Mb.*dx.*dy.*q2.*q3.*rk.*t10.*t12.*t18.*2.4e1+COM_Z.*Jw.*Mb.*dx.*dy.*q1.*q3.*rk.*t10.*t12.*t18.*1.2e1+COM_Z.*Jw.*Mb.*dx.*dy.*q2.*q4.*rk.*t9.*t11.*t18.*1.2e1-COM_Z.*Jw.*Mb.*dx.*dy.*q2.*q4.*rk.*t9.*t12.*t18.*1.2e1-COM_Z.*Jw.*Mb.*dx.*dy.*q2.*q4.*rk.*t10.*t11.*t18.*1.2e1+COM_Z.*Jw.*Mb.*dx.*dy.*q2.*q4.*rk.*t10.*t12.*t18.*1.2e1-COM_X.*Jw.*Mb.*g.*q1.*q4.*t2.*t3.*t8.*t9.*t18.*7.2e1-COM_X.*Jw.*Mb.*g.*q2.*q3.*t2.*t3.*t8.*t9.*t18.*1.2e2+COM_Z.*Jw.*Mb.*g.*q1.*q2.*t2.*t3.*t8.*t9.*t18.*1.68e2-COM_X.*Jw.*Mb.*g.*q1.*q4.*t2.*t3.*t8.*t10.*t18.*1.2e2-COM_X.*Jw.*Mb.*g.*q2.*q3.*t2.*t3.*t8.*t10.*t18.*7.2e1+COM_Z.*Jw.*Mb.*g.*q1.*q2.*t2.*t3.*t8.*t10.*t18.*1.68e2-COM_Z.*Jw.*Mb.*g.*q3.*q4.*t2.*t3.*t8.*t9.*t18.*1.68e2-COM_Z.*Jw.*Mb.*g.*q3.*q4.*t2.*t3.*t8.*t10.*t18.*1.68e2-COM_X.*Jw.*Mb.*g.*q1.*q4.*t2.*t8.*t9.*t10.*t18.*1.2e2-COM_X.*Jw.*Mb.*g.*q2.*q3.*t2.*t8.*t9.*t10.*t18.*7.2e1+COM_Z.*Jw.*Mb.*g.*q1.*q2.*t2.*t8.*t9.*t10.*t18.*1.68e2-COM_X.*Jw.*Mb.*g.*q1.*q4.*t3.*t8.*t9.*t10.*t18.*7.2e1-COM_X.*Jw.*Mb.*g.*q2.*q3.*t3.*t8.*t9.*t10.*t18.*1.2e2+COM_Z.*Jw.*Mb.*g.*q1.*q2.*t3.*t8.*t9.*t10.*t18.*1.68e2-COM_Z.*Jw.*Mb.*g.*q3.*q4.*t2.*t8.*t9.*t10.*t18.*1.68e2-COM_Z.*Jw.*Mb.*g.*q3.*q4.*t3.*t8.*t9.*t10.*t18.*1.68e2-COM_X.*Jw.*Mb.*q2.*q3.*rk.*t2.*t3.*t9.*t17.*t18.*4.8e1+COM_Z.*Jw.*Mb.*q1.*q2.*rk.*t2.*t3.*t9.*t17.*t18.*2.4e1-COM_X.*Jw.*Mb.*q1.*q4.*rk.*t2.*t3.*t10.*t17.*t18.*4.8e1+COM_Z.*Jw.*Mb.*q1.*q2.*rk.*t2.*t3.*t10.*t17.*t18.*2.4e1-COM_Z.*Jw.*Mb.*q3.*q4.*rk.*t2.*t3.*t9.*t17.*t18.*2.4e1-COM_Z.*Jw.*Mb.*q3.*q4.*rk.*t2.*t3.*t10.*t17.*t18.*2.4e1-COM_X.*Jw.*Mb.*q1.*q4.*rk.*t2.*t9.*t10.*t17.*t18.*4.8e1+COM_Z.*Jw.*Mb.*q1.*q2.*rk.*t2.*t9.*t10.*t17.*t18.*2.4e1-COM_X.*Jw.*Mb.*q2.*q3.*rk.*t3.*t9.*t10.*t17.*t18.*4.8e1+COM_Z.*Jw.*Mb.*q1.*q2.*rk.*t3.*t9.*t10.*t17.*t18.*2.4e1-COM_Z.*Jw.*Mb.*q3.*q4.*rk.*t2.*t9.*t10.*t17.*t18.*2.4e1-COM_Z.*Jw.*Mb.*q3.*q4.*rk.*t3.*t9.*t10.*t17.*t18.*2.4e1+COM_Y.*Jw.*Mb.*dx.*dy.*q2.*q3.*rk.*t2.*t3.*t9.*t18.*4.8e1+COM_Z.*Jw.*Mb.*dx.*dy.*q1.*q3.*rk.*t2.*t3.*t9.*t18.*2.4e1-COM_Y.*Jw.*Mb.*dx.*dy.*q1.*q4.*rk.*t2.*t3.*t10.*t18.*4.8e1+COM_Z.*Jw.*Mb.*dx.*dy.*q1.*q3.*rk.*t2.*t3.*t10.*t18.*2.4e1+COM_Z.*Jw.*Mb.*dx.*dy.*q2.*q4.*rk.*t2.*t3.*t9.*t18.*2.4e1+COM_Z.*Jw.*Mb.*dx.*dy.*q2.*q4.*rk.*t2.*t3.*t10.*t18.*2.4e1-COM_Y.*Jw.*Mb.*dx.*dy.*q1.*q4.*rk.*t2.*t9.*t10.*t18.*4.8e1+COM_Z.*Jw.*Mb.*dx.*dy.*q1.*q3.*rk.*t2.*t9.*t10.*t18.*2.4e1+COM_Y.*Jw.*Mb.*dx.*dy.*q2.*q3.*rk.*t3.*t9.*t10.*t18.*4.8e1+COM_Z.*Jw.*Mb.*dx.*dy.*q1.*q3.*rk.*t3.*t9.*t10.*t18.*2.4e1+COM_Z.*Jw.*Mb.*dx.*dy.*q2.*q4.*rk.*t2.*t9.*t10.*t18.*2.4e1+COM_Z.*Jw.*Mb.*dx.*dy.*q2.*q4.*rk.*t3.*t9.*t10.*t18.*2.4e1).*2.0]; 
+  f11 = (((((((((((((((((((q1 * q3 * t5 * t6 * t8 * t13 * -9.0F + q1 * q3 * t5 *
+    t6 * t8 * t14 * 9.0F) + q1 * q3 * t5 * t6 * t8 * t15 * 9.0F) - q2 * q4 * t5 *
+    t6 * t8 * t13 * 9.0F) - q1 * q3 * t5 * t6 * t8 * t16 * 9.0F) + q2 * q4 * t5 *
+                       t6 * t8 * t14 * 9.0F) + q2 * q4 * t5 * t6 * t8 * t15 *
+                      9.0F) - q2 * q4 * t5 * t6 * t8 * t16 * 9.0F) - COM_X * Jk *
+                    Mb * g * t2 * t20 * 8.0F) + COM_X * Jk * Mb * g * t3 * t20 *
+                   8.0F) - COM_X * Jk * Mb * g * t9 * t20 * 8.0F) + COM_X * Jk *
+                 Mb * g * t10 * t20 * 8.0F) - COM_X * g * t2 * t8 * t19 * t20 *
+                8.0F) + COM_X * g * t3 * t8 * t19 * t20 * 8.0F) - COM_X * g * t8
+              * t9 * t19 * t20 * 8.0F) + COM_X * g * t8 * t10 * t19 * t20 * 8.0F)
+            - COM_X * Mb * Mk * g * t2 * t8 * t20 * 8.0F) + COM_X * Mb * Mk * g *
+           t3 * t8 * t20 * 8.0F) - COM_X * Mb * Mk * g * t8 * t9 * t20 * 8.0F) +
+         COM_X * Mb * Mk * g * t8 * t10 * t20 * 8.0F) - Jk * Jw * q1 * q3 * t2 *
+    t6 * t18 * 12.0F;
+  f12 = Jk * Jw * q1 * q3 * t3 * t6 * t18 * 12.0F;
+  f13 = Jk * Jw * q2 * q4 * t2 * t6 * t18 * 12.0F;
+  f14 = Jk * Jw * q2 * q4 * t3 * t6 * t18;
+  f15 = Jk * Jw * q1 * q3 * t6 * t9 * t18;
+  f16 = Jk * Jw * q1 * q3 * t6 * t10;
+  f17 = Jk * Jw * q2 * q4 * t6 * t9;
+  f18 = Jk * Jw * q2 * q4 * t6;
+  f19 = COM_Y * g * q1 * q4 * t8;
+  f20 = COM_Y * g * q2 * q3;
+  f21 = COM_Z * g * q1 * q3;
+  f22 = COM_Z * g * q2;
+  f23 = dx * dy * q1;
+  f24 = dx * dy;
+  f25 = dx * dy;
+  f11 = (((((((((((((((((((((((((((((((((((((((((((((((((f11 + f12) - f13) + f14
+    * 12.0F) + f15 * 12.0F) - f16 * t18 * 12.0F) + f17 * t18 * 12.0F) - f18 *
+    t10 * t18 * 12.0F) + f19 * t19 * t20 * 16.0F) - f20 * t8 * t19 * t20 * 16.0F)
+    - f21 * t8 * t19 * t20 * 16.0F) - f22 * q4 * t8 * t19 * t20 * 16.0F) - f23 *
+    q2 * t5 * t8 * t13 * 9.0F) + f24 * q1 * q2 * t5 * t8 * t14 * 9.0F) + f25 *
+    q1 * q2 * t5 * t8 * t15 * 9.0F) - dx * dy * q1 * q2 * t5 * t8 * t16 * 9.0F)
+    + dx * dy * q3 * q4 * t5 * t8 * t13 * 9.0F) - dx * dy * q3 * q4 * t5 * t8 *
+    t14 * 9.0F) - dx * dy * q3 * q4 * t5 * t8 * t15 * 9.0F) + dx * dy * q3 * q4 *
+    t5 * t8 * t16 * 9.0F) + q1 * q3 * t2 * t4 * t5 * t6 * t8 * 45.0F) + q1 * q3 *
+    t3 * t4 * t5 * t6 * t8 * 9.0F) + q2 * q4 * t2 * t4 * t5 * t6 * t8 * 117.0F)
+    - q1 * q3 * t2 * t5 * t6 * t7 * t8 * 9.0F) + q2 * q4 * t3 * t4 * t5 * t6 *
+    t8 * 9.0F) - q1 * q3 * t3 * t5 * t6 * t7 * t8 * 45.0F) - q2 * q4 * t2 * t5 *
+    t6 * t7 * t8 * 9.0F) - q2 * q4 * t3 * t5 * t6 * t7 * t8 * 117.0F) - q1 * q3 *
+    t2 * t5 * t6 * t8 * t11 * 27.0F) + q1 * q3 * t4 * t5 * t6 * t8 * t9 * 27.0F)
+    + q1 * q3 * t2 * t5 * t6 * t8 * t12 * 9.0F) - q1 * q3 * t3 * t5 * t6 * t8 *
+    t11 * 9.0F) + q1 * q3 * t4 * t5 * t6 * t8 * t10 * 9.0F) + q1 * q3 * t3 * t5 *
+    t6 * t8 * t12 * 27.0F) - q2 * q4 * t2 * t5 * t6 * t8 * t11 * 27.0F) + q2 *
+                       q4 * t4 * t5 * t6 * t8 * t9 * 27.0F) - q1 * q3 * t4 * t5 *
+                      t6 * t8 * t12 * 54.0F) - q1 * q3 * t5 * t6 * t7 * t8 * t9 *
+                     9.0F) + q2 * q4 * t2 * t5 * t6 * t8 * t12 * 9.0F) - q2 * q4
+                   * t3 * t5 * t6 * t8 * t11 * 9.0F) + q2 * q4 * t4 * t5 * t6 *
+                  t8 * t10 * 9.0F) - q1 * q3 * t5 * t6 * t7 * t8 * t10 * 27.0F)
+                + q2 * q4 * t3 * t5 * t6 * t8 * t12 * 27.0F) + q1 * q3 * t5 * t6
+               * t7 * t8 * t11 * 54.0F) - q2 * q4 * t4 * t5 * t6 * t8 * t12 *
+              54.0F) - q2 * q4 * t5 * t6 * t7 * t8 * t9 * 9.0F) - q2 * q4 * t2 *
+            t4 * t5 * t8 * t17 * 72.0F) - q2 * q4 * t5 * t6 * t7 * t8 * t10 *
+           27.0F) - q1 * q3 * t5 * t6 * t8 * t9 * t11 * 117.0F) + q2 * q4 * t5 *
+         t6 * t7 * t8 * t11 * 54.0F) + q1 * q3 * t5 * t6 * t8 * t9 * t12 * 9.0F;
+  f12 = q1 * q3 * t5 * t6 * t8 * t10 * t11 * 9.0F;
+  f13 = q1 * q3 * t5 * t6 * t8 * t10 * t12;
+  f14 = q2 * q4 * t5 * t6 * t8 * t9 * t11;
+  f15 = q2 * q4 * t3 * t5 * t7 * t8;
+  f16 = q2 * q4 * t5 * t6 * t8 * t9;
+  f17 = q2 * q4 * t5 * t6 * t8;
+  f18 = q2 * q4 * t5 * t6 * t8;
+  f19 = q1 * q3 * t2 * t5;
+  f20 = q1 * q3 * t3 * t5;
+  f21 = q1 * q3 * t4;
+  f22 = q1 * q3 * t5;
+  f23 = q2 * q4;
+  f24 = q2 * q4;
+  f11 = ((((((((((((((((((((((((((((((((((((((((((((((((f11 - f12) + f13 *
+    117.0F) - f14 * 45.0F) + f15 * t17 * 72.0F) + f16 * t12 * 9.0F) - f17 * t10 *
+    t11 * 9.0F) + f18 * t10 * t12 * 45.0F) + f19 * t8 * t12 * t17 * 36.0F) - f20
+    * t8 * t11 * t17 * 36.0F) + f21 * t5 * t8 * t10 * t17 * 36.0F) - f22 * t7 *
+    t8 * t9 * t17 * 36.0F) + f23 * t2 * t5 * t8 * t12 * t17 * 36.0F) - f24 * t3 *
+    t5 * t8 * t11 * t17 * 36.0F) + q2 * q4 * t4 * t5 * t8 * t10 * t17 * 36.0F) -
+    q2 * q4 * t5 * t7 * t8 * t9 * t17 * 36.0F) + q1 * q3 * t5 * t8 * t9 * t11 *
+    t17 * 72.0F) - q1 * q3 * t5 * t8 * t10 * t12 * t17 * 72.0F) + COM_Y * Jk *
+    Mb * g * q1 * q4 * t20 * 16.0F) - COM_Y * Jk * Mb * g * q2 * q3 * t20 *
+    16.0F) - COM_Z * Jk * Mb * g * q1 * q3 * t20 * 16.0F) - COM_Z * Jk * Mb * g *
+    q2 * q4 * t20 * 16.0F) - COM_X * Jw * Mb * g * t8 * t13 * t18 * 6.0F) +
+    COM_X * Jw * Mb * g * t8 * t14 * t18 * 6.0F) - COM_X * Jw * Mb * g * t8 *
+    t15 * t18 * 6.0F) + COM_X * Jw * Mb * g * t8 * t16 * t18 * 6.0F) + COM_Y *
+    Mb * Mk * g * q1 * q4 * t8 * t20 * 16.0F) - COM_Y * Mb * Mk * g * q2 * q3 *
+    t8 * t20 * 16.0F) - COM_Z * Mb * Mk * g * q1 * q3 * t8 * t20 * 16.0F) -
+    COM_Z * Mb * Mk * g * q2 * q4 * t8 * t20 * 16.0F) + COM_X * Jw * Mb * g * t2
+    * t4 * t8 * t18 * 30.0F) + COM_X * Jw * Mb * g * t3 * t4 * t8 * t18 * 6.0F)
+    - COM_X * Jw * Mb * g * t2 * t7 * t8 * t18 * 6.0F) - COM_X * Jw * Mb * g *
+                        t3 * t7 * t8 * t18 * 30.0F) + COM_X * Jw * Mb * g * t2 *
+                       t8 * t11 * t18 * 6.0F) + COM_X * Jw * Mb * g * t4 * t8 *
+                      t9 * t18 * 6.0F) - COM_X * Jw * Mb * g * t2 * t8 * t12 *
+                     t18 * 18.0F) + COM_X * Jw * Mb * g * t3 * t8 * t11 * t18 *
+                    18.0F) + COM_X * Jw * Mb * g * t4 * t8 * t10 * t18 * 18.0F)
+                  - COM_X * Jw * Mb * g * t3 * t8 * t12 * t18 * 6.0F) - COM_X *
+                 Jw * Mb * g * t4 * t8 * t11 * t18 * 36.0F) - COM_X * Jw * Mb *
+                g * t7 * t8 * t9 * t18 * 18.0F) - COM_X * Jw * Mb * g * t7 * t8 *
+               t10 * t18 * 6.0F) + COM_X * Jw * Mb * g * t7 * t8 * t12 * t18 *
+              36.0F) + COM_X * Jw * Mb * g * t8 * t9 * t11 * t18 * 30.0F) -
+            COM_X * Jw * Mb * g * t8 * t9 * t12 * t18 * 6.0F) + COM_X * Jw * Mb *
+           g * t8 * t10 * t11 * t18 * 6.0F) - COM_X * Jw * Mb * g * t8 * t10 *
+          t12 * t18 * 30.0F) - Jk * Jw * dx * dy * q1 * q2 * t2 * t18 * 12.0F) +
+    Jk * Jw * dx * dy * q1 * q2 * t3 * t18 * 12.0F;
+  f12 = Jk * Jw * dx * dy * q3 * q4 * t2 * t18 * 12.0F;
+  f13 = Jk * Jw * dx * dy * q3 * q4 * t3 * t18 * 12.0F;
+  f14 = Jk * Jw * dx * dy * q1 * q2 * t9 * t18;
+  f15 = Jk * Jw * dx * dy * q1 * q2 * t10 * t18;
+  f16 = Jk * Jw * dx * dy * q3 * q4 * t9;
+  f17 = Jk * Jw * dx * dy * q3 * q4 * t10;
+  f18 = COM_X * COM_Z * g * rk * t2 * t4;
+  f19 = COM_X * COM_Z * g * rk * t3 * t4;
+  f20 = COM_X * COM_Z * g * rk * t2;
+  f21 = COM_X * COM_Z * g * rk * t3;
+  f22 = COM_X * COM_Z * g * rk;
+  f23 = COM_X * COM_Z * g * rk;
+  f24 = COM_X * COM_Z * g;
+  f25 = COM_X * COM_Z * g;
+  f26 = COM_X * COM_Z;
+  f27 = COM_X * COM_Z;
+  f11 = (((((((((((((((((((((((((((((((((((((((((((((((((f11 + f12) - f13) + f14
+    * 12.0F) - f15 * 12.0F) - f16 * t18 * 12.0F) + f17 * t18 * 12.0F) + f18 *
+    t19 * t20 * 40.0F) - f19 * t19 * t20 * 8.0F) - f20 * t7 * t19 * t20 * 8.0F)
+    + f21 * t7 * t19 * t20 * 40.0F) + f22 * t2 * t11 * t19 * t20 * 8.0F) - f23 *
+    t4 * t9 * t19 * t20 * 8.0F) + f24 * rk * t2 * t12 * t19 * t20 * 8.0F) + f25 *
+    rk * t3 * t11 * t19 * t20 * 8.0F) - f26 * g * rk * t4 * t10 * t19 * t20 *
+    8.0F) + f27 * g * rk * t3 * t12 * t19 * t20 * 8.0F) - COM_X * COM_Z * g * rk
+    * t7 * t9 * t19 * t20 * 8.0F) - COM_X * COM_Z * g * rk * t7 * t10 * t19 *
+    t20 * 8.0F) - COM_X * COM_Z * g * rk * t9 * t11 * t19 * t20 * 40.0F) + COM_X
+    * COM_Z * g * rk * t9 * t12 * t19 * t20 * 8.0F) + COM_X * COM_Z * g * rk *
+    t10 * t11 * t19 * t20 * 8.0F) - COM_X * COM_Z * g * rk * t10 * t12 * t19 *
+    t20 * 40.0F) - Jw * Mb * q1 * q3 * t2 * t6 * t8 * t18 * 12.0F) + Jw * Mb *
+    q1 * q3 * t3 * t6 * t8 * t18 * 12.0F) - Jw * Mb * q2 * q4 * t2 * t6 * t8 *
+    t18 * 12.0F) + Jw * Mb * q2 * q4 * t3 * t6 * t8 * t18 * 12.0F) + Jw * Mb *
+    q1 * q3 * t6 * t8 * t9 * t18 * 12.0F) - Jw * Mb * q1 * q3 * t6 * t8 * t10 *
+    t18 * 12.0F) + Jw * Mb * q2 * q4 * t6 * t8 * t9 * t18 * 12.0F) - Jw * Mb *
+    q2 * q4 * t6 * t8 * t10 * t18 * 12.0F) - Jw * Mk * q1 * q3 * t2 * t6 * t8 *
+    t18 * 12.0F) + Jw * Mk * q1 * q3 * t3 * t6 * t8 * t18 * 12.0F) - Jw * Mk *
+    q2 * q4 * t2 * t6 * t8 * t18 * 12.0F) + Jw * Mk * q2 * q4 * t3 * t6 * t8 *
+                        t18 * 12.0F) + Jw * Mk * q1 * q3 * t6 * t8 * t9 * t18 *
+                       12.0F) - Jw * Mk * q1 * q3 * t6 * t8 * t10 * t18 * 12.0F)
+                     + Jw * Mk * q2 * q4 * t6 * t8 * t9 * t18 * 12.0F) - Jw * Mk
+                    * q2 * q4 * t6 * t8 * t10 * t18 * 12.0F) + dx * dy * q1 * q2
+                   * t2 * t4 * t5 * t8 * 81.0F) + dx * dy * q1 * q2 * t3 * t4 *
+                  t5 * t8 * 9.0F) - dx * dy * q1 * q2 * t2 * t5 * t7 * t8 * 9.0F)
+                - dx * dy * q1 * q2 * t3 * t5 * t7 * t8 * 81.0F) - dx * dy * q3 *
+               q4 * t2 * t4 * t5 * t8 * 81.0F) - dx * dy * q3 * q4 * t3 * t4 *
+              t5 * t8 * 9.0F) - dx * dy * q1 * q2 * t2 * t5 * t8 * t11 * 27.0F)
+            + dx * dy * q1 * q2 * t4 * t5 * t8 * t9 * 27.0F) + dx * dy * q3 * q4
+           * t2 * t5 * t7 * t8 * 9.0F) + dx * dy * q1 * q2 * t2 * t5 * t8 * t12 *
+          9.0F) + dx * dy * q1 * q2 * t3 * t5 * t8 * t11 * 135.0F) - dx * dy *
+    q1 * q2 * t4 * t5 * t8 * t10 * 135.0F;
+  f12 = dx * dy * q3 * q4 * t3 * t5 * t7 * t8 * 81.0F;
+  f13 = dx * dy * q1 * q2 * t3 * t5 * t8 * t12;
+  f14 = dx * dy * q1 * q2 * t4 * t5 * t8 * t12;
+  f15 = dx * dy * q1 * q2 * t5 * t7 * t8;
+  f16 = dx * dy * q1 * q2 * t5 * t7 * t8;
+  f17 = dx * dy * q3 * q4 * t2 * t5;
+  f18 = dx * dy * q3 * q4 * t4 * t5;
+  f19 = dx * dy * q1 * q2 * t5;
+  f20 = dx * dy * q3 * q4 * t2;
+  f21 = dx * dy * q3 * q4;
+  f22 = dx * dy * q3 * q4;
+  f23 = dx * dy * q3;
+  f24 = dx * dy * q1;
+  f25 = dx * dy;
+  f26 = dx * dy;
+  f11 = ((((((((((((((((((((((((((((((((((((((((((((((((f11 + f12) + f13 * 27.0F)
+    - f14 * 54.0F) - f15 * t9 * 9.0F) - f16 * t10 * 27.0F) + f17 * t8 * t11 *
+    27.0F) - f18 * t8 * t9 * 27.0F) + f19 * t7 * t8 * t11 * 54.0F) + f20 * t5 *
+    t8 * t12 * 135.0F) + f21 * t3 * t5 * t8 * t11 * 9.0F) - f22 * t4 * t5 * t8 *
+    t10 * 9.0F) - f23 * q4 * t3 * t5 * t8 * t12 * 27.0F) - f24 * q2 * t5 * t8 *
+    t9 * t11 * 81.0F) + f25 * q3 * q4 * t4 * t5 * t8 * t12 * 54.0F) - f26 * q3 *
+    q4 * t5 * t7 * t8 * t9 * 135.0F) + dx * dy * q1 * q2 * t5 * t8 * t9 * t12 *
+    9.0F) - dx * dy * q1 * q2 * t5 * t8 * t10 * t11 * 9.0F) + dx * dy * q3 * q4 *
+    t5 * t7 * t8 * t10 * 27.0F) + dx * dy * q1 * q2 * t5 * t8 * t10 * t12 *
+    81.0F) - dx * dy * q3 * q4 * t5 * t7 * t8 * t11 * 54.0F) + dx * dy * q3 * q4
+    * t5 * t8 * t9 * t11 * 81.0F) - dx * dy * q3 * q4 * t5 * t8 * t9 * t12 *
+    9.0F) + dx * dy * q3 * q4 * t5 * t8 * t10 * t11 * 9.0F) - dx * dy * q3 * q4 *
+    t5 * t8 * t10 * t12 * 81.0F) - g * q1 * q3 * rk * t4 * t19 * t20 * t21 *
+    16.0F) + g * q1 * q3 * rk * t4 * t19 * t20 * t22 * 16.0F) + g * q2 * q4 * rk
+    * t4 * t19 * t20 * t21 * 16.0F) + g * q1 * q3 * rk * t7 * t19 * t20 * t21 *
+    16.0F) + g * q2 * q4 * rk * t4 * t19 * t20 * t22 * 16.0F) - g * q1 * q3 * rk
+    * t7 * t19 * t20 * t22 * 16.0F) - g * q2 * q4 * rk * t4 * t19 * t20 * t23 *
+    32.0F) - g * q2 * q4 * rk * t7 * t19 * t20 * t21 * 16.0F) - g * q2 * q4 * rk
+                        * t7 * t19 * t20 * t22 * 16.0F) - g * q1 * q3 * rk * t11
+                       * t19 * t20 * t21 * 16.0F) + g * q2 * q4 * rk * t7 * t19 *
+                      t20 * t23 * 32.0F) - g * q1 * q3 * rk * t11 * t19 * t20 *
+                     t22 * 16.0F) + g * q1 * q3 * rk * t12 * t19 * t20 * t21 *
+                    16.0F) + g * q1 * q3 * rk * t11 * t19 * t20 * t23 * 32.0F) +
+                  g * q1 * q3 * rk * t12 * t19 * t20 * t22 * 16.0F) + g * q2 *
+                 q4 * rk * t11 * t19 * t20 * t21 * 16.0F) - g * q1 * q3 * rk *
+                t12 * t19 * t20 * t23 * 32.0F) - g * q2 * q4 * rk * t11 * t19 *
+               t20 * t22 * 16.0F) - g * q2 * q4 * rk * t12 * t19 * t20 * t21 *
+              16.0F) + g * q2 * q4 * rk * t12 * t19 * t20 * t22 * 16.0F) - q1 *
+            q3 * t2 * t3 * t4 * t5 * t6 * t8 * 54.0F) - q2 * q4 * t2 * t3 * t4 *
+           t5 * t6 * t8 * 126.0F) + q1 * q3 * t2 * t3 * t5 * t6 * t7 * t8 *
+          54.0F) + q1 * q3 * t2 * t3 * t5 * t6 * t8 * t9 * 54.0F) + q2 * q4 * t2
+    * t3 * t5 * t6 * t7 * t8 * 126.0F;
+  f12 = q1 * q3 * t2 * t3 * t5 * t6 * t8 * t10 * 54.0F;
+  f13 = q1 * q3 * t2 * t4 * t5 * t6 * t8 * t9 * 126.0F;
+  f14 = q1 * q3 * t2 * t3 * t5 * t6 * t8 * t11;
+  f15 = q1 * q3 * t3 * t4 * t5 * t6 * t8 * t9;
+  f16 = q2 * q4 * t2 * t3 * t5 * t6 * t8;
+  f17 = q1 * q3 * t2 * t3 * t5 * t6 * t8;
+  f18 = q1 * q3 * t3 * t4 * t5 * t6;
+  f19 = q2 * q4 * t2 * t3 * t5 * t6;
+  f20 = q2 * q4 * t2 * t4 * t5;
+  f21 = q1 * q3 * t2 * t5 * t6;
+  f22 = q2 * q4 * t2 * t3;
+  f23 = q2 * q4 * t3 * t4;
+  f24 = q1 * q3 * t2;
+  f25 = q2 * q4 * t2;
+  f26 = q2 * q4;
+  f27 = q1 * q3;
+  f11 = (((((((((((((((((((((((((((((((((((((((((((((((((f11 - f12) - f13) + f14
+    * 90.0F) - f15 * 36.0F) + f16 * t9 * 126.0F) - f17 * t12 * 90.0F) - f18 * t8
+    * t10 * 18.0F) - f19 * t8 * t10 * 126.0F) - f20 * t6 * t8 * t9 * 270.0F) +
+    f21 * t7 * t8 * t9 * 18.0F) + f22 * t5 * t6 * t8 * t11 * 162.0F) - f23 * t5 *
+    t6 * t8 * t9 * 36.0F) + f24 * t5 * t6 * t7 * t8 * t10 * 36.0F) - f25 * t3 *
+    t5 * t6 * t8 * t12 * 162.0F) - f26 * t3 * t4 * t5 * t6 * t8 * t10 * 18.0F) +
+    f27 * t3 * t5 * t6 * t7 * t8 * t10 * 126.0F) + q2 * q4 * t2 * t5 * t6 * t7 *
+    t8 * t9 * 18.0F) - q1 * q3 * t2 * t5 * t6 * t8 * t9 * t10 * 126.0F) + q2 *
+    q4 * t2 * t5 * t6 * t7 * t8 * t10 * 36.0F) + q1 * q3 * t2 * t5 * t6 * t8 *
+    t9 * t11 * 270.0F) + q1 * q3 * t3 * t5 * t6 * t8 * t9 * t10 * 126.0F) + q2 *
+    q4 * t2 * t3 * t4 * t5 * t8 * t17 * 72.0F) + q2 * q4 * t3 * t5 * t6 * t7 *
+    t8 * t10 * 270.0F) - q1 * q3 * t2 * t5 * t6 * t8 * t9 * t12 * 18.0F) + q1 *
+    q3 * t2 * t5 * t6 * t8 * t10 * t11 * 36.0F) - q1 * q3 * t4 * t5 * t6 * t8 *
+    t9 * t10 * 162.0F) - q2 * q4 * t2 * t5 * t6 * t8 * t9 * t10 * 54.0F) - q1 *
+    q3 * t3 * t5 * t6 * t8 * t9 * t12 * 36.0F) + q1 * q3 * t3 * t5 * t6 * t8 *
+    t10 * t11 * 18.0F) + q2 * q4 * t2 * t5 * t6 * t8 * t9 * t11 * 126.0F) + q2 *
+    q4 * t3 * t5 * t6 * t8 * t9 * t10 * 54.0F) + q1 * q3 * t2 * t3 * t5 * t8 *
+    t9 * t17 * 36.0F) - q1 * q3 * t3 * t5 * t6 * t8 * t10 * t12 * 270.0F) - q2 *
+                        q4 * t2 * t3 * t5 * t7 * t8 * t17 * 72.0F) - q2 * q4 *
+                       t2 * t5 * t6 * t8 * t9 * t12 * 18.0F) + q2 * q4 * t2 * t5
+                      * t6 * t8 * t10 * t11 * 36.0F) - q2 * q4 * t4 * t5 * t6 *
+                     t8 * t9 * t10 * 90.0F) - q1 * q3 * t2 * t3 * t5 * t8 * t10 *
+                    t17 * 36.0F) - q1 * q3 * t2 * t4 * t5 * t8 * t9 * t17 *
+                   36.0F) + q1 * q3 * t5 * t6 * t7 * t8 * t9 * t10 * 162.0F) -
+                 q2 * q4 * t3 * t5 * t6 * t8 * t9 * t12 * 36.0F) + q2 * q4 * t3 *
+                t5 * t6 * t8 * t10 * t11 * 18.0F) + q1 * q3 * t2 * t3 * t5 * t8 *
+               t11 * t17 * 72.0F) - q2 * q4 * t2 * t3 * t5 * t8 * t9 * t17 *
+              36.0F) - q2 * q4 * t3 * t5 * t6 * t8 * t10 * t12 * 126.0F) - q1 *
+            q3 * t2 * t3 * t5 * t8 * t12 * t17 * 72.0F) - q1 * q3 * t3 * t4 * t5
+           * t8 * t10 * t17 * 36.0F) + q2 * q4 * t2 * t3 * t5 * t8 * t10 * t17 *
+          36.0F) + q2 * q4 * t2 * t4 * t5 * t8 * t9 * t17 * 108.0F) + q2 * q4 *
+    t5 * t6 * t7 * t8 * t9 * t10 * 90.0F;
+  f12 = q1 * q3 * t2 * t5 * t7 * t8 * t9 * t17 * 36.0F;
+  f13 = q1 * q3 * t5 * t6 * t8 * t9 * t10 * t11;
+  f14 = q2 * q4 * t3 * t4 * t5 * t8 * t10 * t17;
+  f15 = q1 * q3 * t3 * t5 * t7 * t8 * t10;
+  f16 = q1 * q3 * t5 * t6 * t8 * t9 * t10;
+  f17 = q2 * q4 * t2 * t5 * t7 * t8;
+  f18 = q1 * q3 * t2 * t5 * t8 * t9;
+  f19 = q2 * q4 * t5 * t6 * t8;
+  f20 = q1 * q3 * t2 * t5 * t8;
+  f21 = q1 * q3 * t3 * t5;
+  f22 = q2 * q4 * t3 * t5;
+  f23 = q2 * q4 * t5;
+  f24 = q1 * q3 * t2;
+  f25 = q2 * q4;
+  f26 = q1 * q3;
+  f11 = ((((((((((((((((((((((((((((((((((((((((((((((((f11 + f12) + f13 *
+    126.0F) - f14 * 36.0F) + f15 * t17 * 36.0F) - f16 * t12 * 126.0F) + f17 * t9
+    * t17 * 36.0F) + f18 * t10 * t17 * 36.0F) + f19 * t9 * t10 * t11 * 54.0F) -
+    f20 * t9 * t11 * t17 * 108.0F) - f21 * t8 * t9 * t10 * t17 * 36.0F) - f22 *
+    t7 * t8 * t10 * t17 * 108.0F) - f23 * t6 * t8 * t9 * t10 * t12 * 54.0F) -
+    f24 * t5 * t8 * t9 * t12 * t17 * 36.0F) - f25 * t2 * t5 * t8 * t9 * t10 *
+    t17 * 36.0F) + f26 * t3 * t5 * t8 * t10 * t11 * t17 * 36.0F) + q2 * q4 * t2 *
+    t5 * t8 * t9 * t11 * t17 * 36.0F) + q2 * q4 * t3 * t5 * t8 * t9 * t10 * t17 *
+    36.0F) + q1 * q3 * t3 * t5 * t8 * t10 * t12 * t17 * 108.0F) - q2 * q4 * t2 *
+    t5 * t8 * t9 * t12 * t17 * 36.0F) - q2 * q4 * t4 * t5 * t8 * t9 * t10 * t17 *
+    72.0F) + q2 * q4 * t3 * t5 * t8 * t10 * t11 * t17 * 36.0F) - q2 * q4 * t3 *
+    t5 * t8 * t10 * t12 * t17 * 36.0F) + q2 * q4 * t5 * t7 * t8 * t9 * t10 * t17
+    * 72.0F) - q1 * q3 * t5 * t8 * t9 * t10 * t11 * t17 * 72.0F) + q1 * q3 * t5 *
+    t8 * t9 * t10 * t12 * t17 * 72.0F) + COM_X * COM_Z * g * rk * t2 * t3 * t9 *
+    t19 * t20 * 48.0F) + COM_X * COM_Z * g * rk * t2 * t3 * t10 * t19 * t20 *
+    48.0F) - COM_X * COM_Z * g * rk * t2 * t9 * t10 * t19 * t20 * 48.0F) - COM_X
+    * COM_Z * g * rk * t3 * t9 * t10 * t19 * t20 * 48.0F) - dx * dy * q1 * q2 *
+    t2 * t3 * t4 * t5 * t8 * 90.0F) + dx * dy * q1 * q2 * t2 * t3 * t5 * t7 * t8
+    * 90.0F) + dx * dy * q3 * q4 * t2 * t3 * t4 * t5 * t8 * 90.0F) + dx * dy *
+                        q1 * q2 * t2 * t3 * t5 * t8 * t9 * 90.0F) + dx * dy * q1
+                       * q2 * t2 * t3 * t5 * t8 * t10 * 54.0F) - dx * dy * q1 *
+                      q2 * t2 * t4 * t5 * t8 * t9 * 198.0F) - dx * dy * q1 * q2 *
+                     t2 * t3 * t5 * t8 * t11 * 162.0F) - dx * dy * q1 * q2 * t3 *
+                    t4 * t5 * t8 * t9 * 36.0F) - dx * dy * q3 * q4 * t2 * t3 *
+                   t5 * t7 * t8 * 90.0F) - dx * dy * q1 * q2 * t2 * t3 * t5 * t8
+                  * t12 * 126.0F) + dx * dy * q1 * q2 * t3 * t4 * t5 * t8 * t10 *
+                 126.0F) + dx * dy * q1 * q2 * t2 * t5 * t7 * t8 * t9 * 18.0F) +
+               dx * dy * q3 * q4 * t2 * t3 * t5 * t8 * t9 * 54.0F) + dx * dy *
+              q1 * q2 * t2 * t5 * t7 * t8 * t10 * 36.0F) + dx * dy * q3 * q4 *
+             t2 * t3 * t5 * t8 * t10 * 90.0F) + dx * dy * q3 * q4 * t2 * t4 * t5
+            * t8 * t9 * 54.0F) + dx * dy * q1 * q2 * t3 * t5 * t7 * t8 * t10 *
+           54.0F) - dx * dy * q3 * q4 * t2 * t3 * t5 * t8 * t11 * 126.0F) + dx *
+         dy * q3 * q4 * t3 * t4 * t5 * t8 * t9 * 36.0F) - dx * dy * q1 * q2 * t2
+    * t5 * t8 * t9 * t10 * 90.0F;
+  f12 = dx * dy * q3 * q4 * t2 * t3 * t5 * t8 * t12 * 162.0F;
+  f13 = dx * dy * q3 * q4 * t3 * t4 * t5 * t8 * t10 * 18.0F;
+  f14 = dx * dy * q1 * q2 * t2 * t5 * t8 * t9 * t11;
+  f15 = dx * dy * q1 * q2 * t3 * t5 * t8 * t9 * t10;
+  f16 = dx * dy * q3 * q4 * t2 * t5 * t7 * t8;
+  f17 = dx * dy * q1 * q2 * t2 * t5 * t8 * t9;
+  f18 = dx * dy * q1 * q2 * t2 * t5 * t8;
+  f19 = dx * dy * q1 * q2 * t4 * t5 * t8;
+  f20 = dx * dy * q3 * q4 * t2 * t5;
+  f21 = dx * dy * q1 * q2 * t3 * t5;
+  f22 = dx * dy * q1 * q2 * t3;
+  f23 = dx * dy * q3 * q4 * t3;
+  f24 = dx * dy * q1 * q2;
+  f25 = dx * dy * q3 * q4;
+  f26 = dx * dy * q1;
+  f27 = dx * dy * q3;
+  f28 = dx * dy;
+  f29 = dx * dy;
+  f11 = (((((((((((((((((((((((((((((((((((((((((((((((((f11 - f12) + f13) + f14
+    * 198.0F) - f15 * 54.0F) + f16 * t9 * 126.0F) - f17 * t12 * 18.0F) + f18 *
+    t10 * t11 * 36.0F) + f19 * t9 * t10 * 162.0F) - f20 * t7 * t8 * t10 * 36.0F)
+    - f21 * t8 * t9 * t12 * 36.0F) - f22 * t5 * t8 * t10 * t11 * 126.0F) - f23 *
+    t5 * t7 * t8 * t10 * 198.0F) - f24 * t3 * t5 * t8 * t10 * t12 * 54.0F) - f25
+    * t2 * t5 * t8 * t9 * t10 * 54.0F) + f26 * q2 * t5 * t7 * t8 * t9 * t10 *
+    126.0F) - f27 * q4 * t2 * t5 * t8 * t9 * t11 * 54.0F) - f28 * q3 * q4 * t3 *
+    t5 * t8 * t9 * t10 * 90.0F) - f29 * q3 * q4 * t2 * t5 * t8 * t9 * t12 *
+    126.0F) - dx * dy * q3 * q4 * t2 * t5 * t8 * t10 * t11 * 36.0F) + dx * dy *
+    q3 * q4 * t4 * t5 * t8 * t9 * t10 * 126.0F) + dx * dy * q3 * q4 * t3 * t5 *
+    t8 * t9 * t12 * 36.0F) - dx * dy * q3 * q4 * t3 * t5 * t8 * t10 * t11 *
+    18.0F) + dx * dy * q3 * q4 * t3 * t5 * t8 * t10 * t12 * 198.0F) + dx * dy *
+    q1 * q2 * t5 * t8 * t9 * t10 * t11 * 90.0F) + dx * dy * q3 * q4 * t5 * t7 *
+    t8 * t9 * t10 * 162.0F) - dx * dy * q1 * q2 * t5 * t8 * t9 * t10 * t12 *
+    90.0F) - dx * dy * q3 * q4 * t5 * t8 * t9 * t10 * t11 * 90.0F) + dx * dy *
+    q3 * q4 * t5 * t8 * t9 * t10 * t12 * 90.0F) + g * q1 * q3 * rk * t2 * t9 *
+    t19 * t20 * t21 * 32.0F) - g * q1 * q3 * rk * t2 * t9 * t19 * t20 * t23 *
+    32.0F) - g * q1 * q3 * rk * t2 * t10 * t19 * t20 * t22 * 32.0F) + g * q1 *
+    q3 * rk * t3 * t9 * t19 * t20 * t22 * 32.0F) - g * q1 * q3 * rk * t3 * t10 *
+    t19 * t20 * t21 * 32.0F) - g * q2 * q4 * rk * t2 * t9 * t19 * t20 * t21 *
+                        32.0F) + g * q1 * q3 * rk * t2 * t10 * t19 * t20 * t23 *
+                       32.0F) - g * q1 * q3 * rk * t3 * t9 * t19 * t20 * t23 *
+                      32.0F) + g * q1 * q3 * rk * t3 * t10 * t19 * t20 * t23 *
+                     32.0F) + g * q2 * q4 * rk * t2 * t9 * t19 * t20 * t23 *
+                    32.0F) - g * q2 * q4 * rk * t2 * t10 * t19 * t20 * t22 *
+                   32.0F) + g * q2 * q4 * rk * t3 * t9 * t19 * t20 * t22 * 32.0F)
+                 + g * q2 * q4 * rk * t3 * t10 * t19 * t20 * t21 * 32.0F) + g *
+                q2 * q4 * rk * t2 * t10 * t19 * t20 * t23 * 32.0F) - g * q2 * q4
+               * rk * t3 * t9 * t19 * t20 * t23 * 32.0F) - g * q2 * q4 * rk * t3
+              * t10 * t19 * t20 * t23 * 32.0F) - COM_Y * Jw * Mb * g * q1 * q4 *
+             t4 * t8 * t18 * 12.0F) - COM_Y * Jw * Mb * g * q2 * q3 * t4 * t8 *
+            t18 * 12.0F) - COM_Z * Jw * Mb * g * q1 * q3 * t4 * t8 * t18 * 12.0F)
+          - COM_Z * Jw * Mb * g * q2 * q4 * t4 * t8 * t18 * 12.0F) + COM_Y * Jw *
+         Mb * g * q1 * q4 * t7 * t8 * t18 * 12.0F) + COM_Y * Jw * Mb * g * q2 *
+    q3 * t7 * t8 * t18 * 12.0F;
+  f12 = COM_Z * Jw * Mb * g * q1 * q3 * t7 * t8 * t18 * 12.0F;
+  f13 = COM_Z * Jw * Mb * g * q2 * q4 * t7 * t8 * t18;
+  f14 = COM_Y * Jw * Mb * g * q1 * q4 * t8 * t11 * t18;
+  f15 = COM_Y * Jw * Mb * g * q2 * q3 * t8 * t11;
+  f16 = COM_Z * Jw * Mb * g * q1 * q3 * t8 * t11;
+  f17 = COM_Y * Jw * Mb * g * q1 * q4 * t8;
+  f18 = COM_Y * Jw * Mb * g * q2 * q3 * t8;
+  f19 = COM_Z * Jw * Mb * g * q1 * q3;
+  f20 = COM_Z * Jw * Mb * g * q2 * q4;
+  f21 = COM_Z * Jw * Mb * g * q2;
+  f22 = COM_X * Jw * Mb * g * t2;
+  f23 = COM_X * Jw * Mb * g;
+  f24 = COM_X * Jw * Mb * g;
+  f25 = COM_X * Jw * Mb;
+  f26 = COM_X * Jw * Mb;
+  f27 = COM_X * Jw;
+  f28 = COM_X * Jw;
+  f11 = ((((((((((((((((((((((((((((((((((((((((((((((((f11 - f12) - f13 * 12.0F)
+    + f14 * 12.0F) + f15 * t18 * 12.0F) - f16 * t18 * 12.0F) - f17 * t12 * t18 *
+    12.0F) - f18 * t12 * t18 * 12.0F) - f19 * t8 * t12 * t18 * 12.0F) - f20 * t8
+    * t11 * t18 * 12.0F) - f21 * q4 * t8 * t12 * t18 * 12.0F) - f22 * t3 * t4 *
+    t8 * t18 * 36.0F) + f23 * t2 * t3 * t7 * t8 * t18 * 36.0F) + f24 * t2 * t3 *
+    t8 * t9 * t18 * 12.0F) - f25 * g * t2 * t3 * t8 * t10 * t18 * 12.0F) - f26 *
+    g * t2 * t4 * t8 * t9 * t18 * 48.0F) - f27 * Mb * g * t2 * t3 * t8 * t11 *
+    t18 * 12.0F) - f28 * Mb * g * t2 * t4 * t8 * t10 * t18 * 36.0F) - COM_X * Jw
+    * Mb * g * t3 * t4 * t8 * t9 * t18 * 12.0F) + COM_X * Jw * Mb * g * t2 * t3 *
+    t8 * t12 * t18 * 12.0F) - COM_X * Jw * Mb * g * t3 * t4 * t8 * t10 * t18 *
+    24.0F) + COM_X * Jw * Mb * g * t2 * t7 * t8 * t9 * t18 * 24.0F) + COM_X * Jw
+    * Mb * g * t2 * t7 * t8 * t10 * t18 * 12.0F) + COM_X * Jw * Mb * g * t3 * t7
+    * t8 * t9 * t18 * 36.0F) + COM_X * Jw * Mb * g * t3 * t7 * t8 * t10 * t18 *
+    48.0F) + COM_X * Jw * Mb * g * t2 * t8 * t9 * t10 * t18 * 12.0F) - COM_X *
+    Jw * Mb * g * t2 * t8 * t9 * t11 * t18 * 48.0F) - COM_X * Jw * Mb * g * t3 *
+    t8 * t9 * t10 * t18 * 12.0F) + COM_X * Jw * Mb * g * t2 * t8 * t9 * t12 *
+    t18 * 24.0F) - COM_X * Jw * Mb * g * t2 * t8 * t10 * t11 * t18 * 12.0F) -
+    COM_X * Jw * Mb * g * t3 * t8 * t9 * t11 * t18 * 36.0F) - COM_X * Jw * Mb *
+    g * t4 * t8 * t9 * t10 * t18 * 12.0F) + COM_X * Jw * Mb * g * t2 * t8 * t10 *
+    t12 * t18 * 36.0F) + COM_X * Jw * Mb * g * t3 * t8 * t9 * t12 * t18 * 12.0F)
+                       - COM_X * Jw * Mb * g * t3 * t8 * t10 * t11 * t18 * 24.0F)
+                      + COM_X * Jw * Mb * g * t3 * t8 * t10 * t12 * t18 * 48.0F)
+                     + COM_X * Jw * Mb * g * t7 * t8 * t9 * t10 * t18 * 12.0F) -
+                    COM_X * Jw * Mb * g * t8 * t9 * t10 * t11 * t18 * 36.0F) +
+                   COM_X * Jw * Mb * g * t8 * t9 * t10 * t12 * t18 * 36.0F) -
+                  COM_X * Jw * Mb * rk * t2 * t3 * t4 * t6 * t18 * 24.0F) +
+                 COM_X * Jw * Mb * rk * t2 * t3 * t6 * t7 * t18 * 24.0F) - COM_X
+                * Jw * Mb * rk * t2 * t4 * t6 * t9 * t18 * 48.0F) + COM_X * Jw *
+               Mb * rk * t2 * t3 * t6 * t11 * t18 * 24.0F) - COM_X * Jw * Mb *
+              rk * t2 * t3 * t6 * t12 * t18 * 24.0F) + COM_X * Jw * Mb * rk * t3
+             * t6 * t7 * t10 * t18 * 48.0F) - COM_X * Jw * Mb * rk * t2 * t6 *
+            t9 * t11 * t18 * 48.0F) + COM_X * Jw * Mb * rk * t4 * t6 * t9 * t10 *
+           t18 * 24.0F) + COM_X * Jw * Mb * rk * t3 * t6 * t10 * t12 * t18 *
+          48.0F) - COM_X * Jw * Mb * rk * t6 * t7 * t9 * t10 * t18 * 24.0F) -
+    COM_X * Jw * Mb * rk * t6 * t9 * t10 * t11 * t18 * 24.0F;
+  f12 = COM_X * Jw * Mb * rk * t6 * t9 * t10 * t12 * t18 * 24.0F;
+  f13 = COM_X * COM_Y * g * q1 * q2 * rk * t4 * t19 * t20 * 48.0F;
+  f14 = COM_X * COM_Y * g * q1 * q2 * rk * t7 * t19 * t20;
+  f15 = COM_X * COM_Y * g * q3 * q4 * rk * t4 * t19 * t20;
+  f16 = COM_Y * COM_Z * g * q1 * q4 * rk * t4 * t19;
+  f17 = COM_Y * COM_Z * g * q2 * q3 * rk * t4 * t19;
+  f18 = COM_X * COM_Y * g * q1 * q2 * rk * t11;
+  f19 = COM_X * COM_Y * g * q3 * q4 * rk * t7;
+  f20 = COM_Y * COM_Z * g * q1 * q4 * rk;
+  f21 = COM_Y * COM_Z * g * q2 * q3 * rk;
+  f22 = COM_X * COM_Y * g * q1 * q2;
+  f23 = COM_X * COM_Y * g * q3 * q4;
+  f24 = COM_Y * COM_Z * g * q1;
+  f25 = COM_Y * COM_Z * g * q2;
+  f26 = COM_X * COM_Y * g;
+  f27 = COM_Y * COM_Z * g;
+  f28 = COM_Y * COM_Z;
+  f29 = Jw * Mb;
+  f11 = (((((((((((((((((((((((((((((((((((((((((((((((((f11 + f12) + f13) - f14
+    * 16.0F) + f15 * 16.0F) - f16 * t20 * 48.0F) + f17 * t20 * 16.0F) + f18 *
+    t19 * t20 * 48.0F) - f19 * t19 * t20 * 48.0F) + f20 * t7 * t19 * t20 * 16.0F)
+    - f21 * t7 * t19 * t20 * 48.0F) - f22 * rk * t12 * t19 * t20 * 16.0F) + f23 *
+    rk * t11 * t19 * t20 * 16.0F) + f24 * q4 * rk * t11 * t19 * t20 * 16.0F) -
+    f25 * q3 * rk * t11 * t19 * t20 * 48.0F) - f26 * q3 * q4 * rk * t12 * t19 *
+    t20 * 48.0F) - f27 * q1 * q4 * rk * t12 * t19 * t20 * 48.0F) + f28 * g * q2 *
+    q3 * rk * t12 * t19 * t20 * 16.0F) - f29 * dx * dy * q1 * q2 * t2 * t8 * t18
+    * 12.0F) + Jw * Mb * dx * dy * q1 * q2 * t3 * t8 * t18 * 12.0F) + Jw * Mb *
+    dx * dy * q3 * q4 * t2 * t8 * t18 * 12.0F) - Jw * Mb * dx * dy * q3 * q4 *
+    t3 * t8 * t18 * 12.0F) + Jw * Mb * dx * dy * q1 * q2 * t8 * t9 * t18 * 12.0F)
+    - Jw * Mb * dx * dy * q1 * q2 * t8 * t10 * t18 * 12.0F) - Jw * Mb * dx * dy *
+    q3 * q4 * t8 * t9 * t18 * 12.0F) + Jw * Mb * dx * dy * q3 * q4 * t8 * t10 *
+    t18 * 12.0F) - Jw * Mk * dx * dy * q1 * q2 * t2 * t8 * t18 * 12.0F) + Jw *
+    Mk * dx * dy * q1 * q2 * t3 * t8 * t18 * 12.0F) + Jw * Mk * dx * dy * q3 *
+    q4 * t2 * t8 * t18 * 12.0F) - Jw * Mk * dx * dy * q3 * q4 * t3 * t8 * t18 *
+    12.0F) + Jw * Mk * dx * dy * q1 * q2 * t8 * t9 * t18 * 12.0F) - Jw * Mk * dx
+    * dy * q1 * q2 * t8 * t10 * t18 * 12.0F) - Jw * Mk * dx * dy * q3 * q4 * t8 *
+    t9 * t18 * 12.0F) + Jw * Mk * dx * dy * q3 * q4 * t8 * t10 * t18 * 12.0F) +
+                        COM_Y * Jw * Mb * dx * dy * rk * t2 * t4 * t9 * t18 *
+                        24.0F) + COM_Y * Jw * Mb * dx * dy * rk * t2 * t3 * t11 *
+                       t18 * 48.0F) + COM_Y * Jw * Mb * dx * dy * rk * t2 * t3 *
+                      t12 * t18 * 48.0F) - COM_Y * Jw * Mb * dx * dy * rk * t3 *
+                     t4 * t10 * t18 * 24.0F) - COM_Y * Jw * Mb * dx * dy * rk *
+                    t2 * t7 * t9 * t18 * 24.0F) + COM_Y * Jw * Mb * dx * dy * rk
+                   * t3 * t7 * t10 * t18 * 24.0F) - COM_Y * Jw * Mb * dx * dy *
+                  rk * t2 * t9 * t11 * t18 * 24.0F) + COM_Y * Jw * Mb * dx * dy *
+                 rk * t2 * t9 * t12 * t18 * 24.0F) - COM_Y * Jw * Mb * dx * dy *
+                rk * t4 * t9 * t10 * t18 * 48.0F) + COM_Y * Jw * Mb * dx * dy *
+               rk * t3 * t10 * t11 * t18 * 24.0F) - COM_Y * Jw * Mb * dx * dy *
+              rk * t3 * t10 * t12 * t18 * 24.0F) - COM_Y * Jw * Mb * dx * dy *
+             rk * t7 * t9 * t10 * t18 * 48.0F) + COM_Y * Jw * Mb * g * q1 * q4 *
+            t2 * t3 * t8 * t18 * 48.0F) - COM_Y * Jw * Mb * g * q2 * q3 * t2 *
+           t3 * t8 * t18 * 48.0F) - COM_Z * Jw * Mb * g * q1 * q3 * t2 * t3 * t8
+          * t18 * 72.0F) - COM_Y * Jw * Mb * g * q1 * q4 * t2 * t4 * t8 * t18 *
+         36.0F) + COM_Y * Jw * Mb * g * q2 * q3 * t2 * t4 * t8 * t18 * 60.0F;
+  f12 = COM_Z * Jw * Mb * g * q1 * q3 * t2 * t4 * t8 * t18 * 84.0F;
+  f13 = COM_Y * Jw * Mb * g * q1 * q4 * t3 * t4 * t8 * t18;
+  f14 = COM_Y * Jw * Mb * g * q2 * q3 * t3 * t4 * t8 * t18;
+  f15 = COM_Z * Jw * Mb * g * q1 * q3 * t3 * t4 * t8;
+  f16 = COM_Z * Jw * Mb * g * q2 * q4 * t2 * t3 * t8;
+  f17 = COM_Z * Jw * Mb * g * q2 * q4 * t2 * t4;
+  f18 = COM_Y * Jw * Mb * g * q1 * q4 * t2 * t7;
+  f19 = COM_Y * Jw * Mb * g * q2 * q3 * t2;
+  f20 = COM_Z * Jw * Mb * g * q1 * q3 * t2;
+  f21 = COM_Z * Jw * Mb * g * q2 * q4;
+  f22 = COM_Y * Jw * Mb * g * q1 * q4;
+  f23 = COM_Y * Jw * Mb * g * q2;
+  f24 = COM_Z * Jw * Mb * g * q1;
+  f25 = COM_Y * Jw * Mb * g;
+  f26 = COM_Y * Jw * Mb * g;
+  f27 = COM_Z * Jw * Mb;
+  f28 = COM_Z * Jw * Mb;
+  f29 = COM_Y * Jw;
+  f30 = COM_Y * Jw;
+  f11 = ((((((((((((((((((((((((((((((((((((((((((((((((f11 + f12) + f13 * 12.0F)
+    + f14 * 12.0F) + f15 * t18 * 12.0F) - f16 * t18 * 72.0F) + f17 * t8 * t18 *
+    84.0F) - f18 * t8 * t18 * 12.0F) - f19 * t7 * t8 * t18 * 12.0F) + f20 * t7 *
+    t8 * t18 * 12.0F) + f21 * t3 * t4 * t8 * t18 * 12.0F) - f22 * t3 * t7 * t8 *
+    t18 * 60.0F) + f23 * q3 * t3 * t7 * t8 * t18 * 36.0F) + f24 * q3 * t3 * t7 *
+    t8 * t18 * 84.0F) + f25 * q1 * q4 * t2 * t8 * t9 * t18 * 48.0F) - f26 * q2 *
+    q3 * t2 * t8 * t9 * t18 * 48.0F) - f27 * g * q1 * q3 * t2 * t8 * t9 * t18 *
+    72.0F) + f28 * g * q2 * q4 * t2 * t7 * t8 * t18 * 12.0F) + f29 * Mb * g * q1
+    * q4 * t2 * t8 * t10 * t18 * 24.0F) - f30 * Mb * g * q1 * q4 * t3 * t8 * t9 *
+    t18 * 24.0F) + COM_Y * Jw * Mb * g * q2 * q3 * t2 * t8 * t10 * t18 * 24.0F)
+    - COM_Y * Jw * Mb * g * q2 * q3 * t3 * t8 * t9 * t18 * 24.0F) - COM_Z * Jw *
+    Mb * g * q1 * q3 * t2 * t8 * t10 * t18 * 24.0F) - COM_Z * Jw * Mb * g * q1 *
+    q3 * t3 * t8 * t9 * t18 * 24.0F) + COM_Z * Jw * Mb * g * q2 * q4 * t3 * t7 *
+    t8 * t18 * 84.0F) - COM_Y * Jw * Mb * g * q1 * q4 * t2 * t8 * t11 * t18 *
+    36.0F) + COM_Y * Jw * Mb * g * q1 * q4 * t3 * t8 * t10 * t18 * 48.0F) +
+    COM_Y * Jw * Mb * g * q1 * q4 * t4 * t8 * t9 * t18 * 36.0F) - COM_Y * Jw *
+    Mb * g * q2 * q3 * t2 * t8 * t11 * t18 * 36.0F) - COM_Y * Jw * Mb * g * q2 *
+    q3 * t3 * t8 * t10 * t18 * 48.0F) + COM_Y * Jw * Mb * g * q2 * q3 * t4 * t8 *
+    t9 * t18 * 36.0F) + COM_Z * Jw * Mb * g * q1 * q3 * t2 * t8 * t11 * t18 *
+    36.0F) - COM_Z * Jw * Mb * g * q1 * q3 * t3 * t8 * t10 * t18 * 72.0F) +
+                        COM_Z * Jw * Mb * g * q1 * q3 * t4 * t8 * t9 * t18 *
+                        36.0F) - COM_Z * Jw * Mb * g * q2 * q4 * t2 * t8 * t9 *
+                       t18 * 72.0F) - COM_Y * Jw * Mb * g * q1 * q4 * t2 * t8 *
+                      t12 * t18 * 36.0F) - COM_Y * Jw * Mb * g * q1 * q4 * t3 *
+                     t8 * t11 * t18 * 60.0F) - COM_Y * Jw * Mb * g * q1 * q4 *
+                    t4 * t8 * t10 * t18 * 36.0F) + COM_Y * Jw * Mb * g * q2 * q3
+                   * t2 * t8 * t12 * t18 * 60.0F) + COM_Y * Jw * Mb * g * q2 *
+                  q3 * t3 * t8 * t11 * t18 * 36.0F) + COM_Y * Jw * Mb * g * q2 *
+                 q3 * t4 * t8 * t10 * t18 * 60.0F) + COM_Z * Jw * Mb * g * q1 *
+                q3 * t2 * t8 * t12 * t18 * 84.0F) + COM_Z * Jw * Mb * g * q1 *
+               q3 * t3 * t8 * t11 * t18 * 84.0F) + COM_Z * Jw * Mb * g * q1 * q3
+              * t4 * t8 * t10 * t18 * 84.0F) - COM_Z * Jw * Mb * g * q2 * q4 *
+             t2 * t8 * t10 * t18 * 24.0F) - COM_Z * Jw * Mb * g * q2 * q4 * t3 *
+            t8 * t9 * t18 * 24.0F) + COM_Y * Jw * Mb * g * q1 * q4 * t3 * t8 *
+           t12 * t18 * 36.0F) + COM_Y * Jw * Mb * g * q2 * q3 * t3 * t8 * t12 *
+          t18 * 36.0F) + COM_Z * Jw * Mb * g * q1 * q3 * t3 * t8 * t12 * t18 *
+         36.0F) + COM_Z * Jw * Mb * g * q2 * q4 * t2 * t8 * t11 * t18 * 36.0F;
+  f12 = COM_Z * Jw * Mb * g * q2 * q4 * t3 * t8 * t10 * t18 * 72.0F;
+  f13 = COM_Z * Jw * Mb * g * q2 * q4 * t4 * t8 * t9 * t18 * 36.0F;
+  f14 = COM_Y * Jw * Mb * g * q1 * q4 * t7 * t8 * t9 * t18;
+  f15 = COM_Y * Jw * Mb * g * q2 * q3 * t7 * t8 * t9 * t18;
+  f16 = COM_Z * Jw * Mb * g * q1 * q3 * t7 * t8 * t9;
+  f17 = COM_Z * Jw * Mb * g * q2 * q4 * t2 * t8 * t12;
+  f18 = COM_Z * Jw * Mb * g * q2 * q4 * t3 * t8;
+  f19 = COM_Z * Jw * Mb * g * q2 * q4 * t4 * t8;
+  f20 = COM_Y * Jw * Mb * g * q1 * q4 * t7;
+  f21 = COM_Y * Jw * Mb * g * q2 * q3 * t7;
+  f22 = COM_Z * Jw * Mb * g * q1 * q3;
+  f23 = COM_Z * Jw * Mb * g * q2 * q4;
+  f24 = COM_Z * Jw * Mb * g * q2;
+  f25 = COM_Y * Jw * Mb * g * q1;
+  f26 = COM_Y * Jw * Mb * g;
+  f27 = COM_Z * Jw * Mb * g;
+  f28 = COM_Z * Jw * Mb;
+  f29 = COM_Y * Jw * Mb;
+  f30 = COM_Y * Jw;
+  f31 = COM_Z * Jw;
+  f11 = (((((((((((((((((((((((((((((((((((((((((((((((((f11 - f12) + f13) - f14
+    * 60.0F) + f15 * 36.0F) + f16 * t18 * 84.0F) + f17 * t18 * 84.0F) + f18 *
+    t11 * t18 * 84.0F) + f19 * t10 * t18 * 84.0F) - f20 * t8 * t10 * t18 * 36.0F)
+    - f21 * t8 * t10 * t18 * 36.0F) + f22 * t7 * t8 * t10 * t18 * 36.0F) + f23 *
+    t3 * t8 * t12 * t18 * 36.0F) + f24 * q4 * t7 * t8 * t9 * t18 * 84.0F) + f25 *
+    q4 * t8 * t9 * t10 * t18 * 48.0F) - f26 * q2 * q3 * t8 * t9 * t10 * t18 *
+    48.0F) - f27 * q1 * q3 * t8 * t9 * t10 * t18 * 72.0F) + f28 * g * q2 * q4 *
+    t7 * t8 * t10 * t18 * 36.0F) - f29 * g * q1 * q4 * t8 * t9 * t11 * t18 *
+    60.0F) + f30 * Mb * g * q2 * q3 * t8 * t9 * t11 * t18 * 36.0F) + f31 * Mb *
+    g * q1 * q3 * t8 * t9 * t11 * t18 * 84.0F) + COM_Y * Jw * Mb * g * q1 * q4 *
+    t8 * t9 * t12 * t18 * 12.0F) - COM_Y * Jw * Mb * g * q1 * q4 * t8 * t10 *
+    t11 * t18 * 12.0F) + COM_Y * Jw * Mb * g * q2 * q3 * t8 * t9 * t12 * t18 *
+    12.0F) - COM_Y * Jw * Mb * g * q2 * q3 * t8 * t10 * t11 * t18 * 12.0F) +
+    COM_Z * Jw * Mb * g * q1 * q3 * t8 * t9 * t12 * t18 * 12.0F) + COM_Z * Jw *
+    Mb * g * q1 * q3 * t8 * t10 * t11 * t18 * 12.0F) - COM_Z * Jw * Mb * g * q2 *
+    q4 * t8 * t9 * t10 * t18 * 72.0F) - COM_Y * Jw * Mb * g * q1 * q4 * t8 * t10
+    * t12 * t18 * 36.0F) + COM_Y * Jw * Mb * g * q2 * q3 * t8 * t10 * t12 * t18 *
+    60.0F) + COM_Z * Jw * Mb * g * q1 * q3 * t8 * t10 * t12 * t18 * 84.0F) +
+    COM_Z * Jw * Mb * g * q2 * q4 * t8 * t9 * t11 * t18 * 84.0F) + COM_Z * Jw *
+    Mb * g * q2 * q4 * t8 * t9 * t12 * t18 * 12.0F) + COM_Z * Jw * Mb * g * q2 *
+    q4 * t8 * t10 * t11 * t18 * 12.0F) + COM_Z * Jw * Mb * g * q2 * q4 * t8 *
+                        t10 * t12 * t18 * 84.0F) + COM_Y * Jw * Mb * q2 * q3 *
+                       rk * t2 * t4 * t6 * t18 * 24.0F) + COM_Z * Jw * Mb * q1 *
+                      q3 * rk * t2 * t4 * t6 * t18 * 12.0F) + COM_Y * Jw * Mb *
+                     q1 * q4 * rk * t3 * t4 * t6 * t18 * 24.0F) - COM_Z * Jw *
+                    Mb * q1 * q3 * rk * t3 * t4 * t6 * t18 * 12.0F) + COM_Z * Jw
+                   * Mb * q2 * q4 * rk * t2 * t4 * t6 * t18 * 12.0F) - COM_Y *
+                  Jw * Mb * q2 * q3 * rk * t2 * t6 * t7 * t18 * 24.0F) - COM_Z *
+                 Jw * Mb * q1 * q3 * rk * t2 * t6 * t7 * t18 * 12.0F) - COM_Z *
+                Jw * Mb * q2 * q4 * rk * t3 * t4 * t6 * t18 * 12.0F) - COM_Y *
+               Jw * Mb * q1 * q4 * rk * t3 * t6 * t7 * t18 * 24.0F) + COM_Z * Jw
+              * Mb * q1 * q3 * rk * t3 * t6 * t7 * t18 * 12.0F) - COM_Z * Jw *
+             Mb * q2 * q4 * rk * t2 * t6 * t7 * t18 * 12.0F) + COM_Z * Jw * Mb *
+            q2 * q4 * rk * t3 * t6 * t7 * t18 * 12.0F) + COM_Y * Jw * Mb * q1 *
+           q4 * rk * t4 * t6 * t9 * t18 * 72.0F) - COM_Y * Jw * Mb * q2 * q3 *
+          rk * t2 * t6 * t11 * t18 * 72.0F) - COM_Z * Jw * Mb * q1 * q3 * rk *
+         t2 * t6 * t11 * t18 * 36.0F) - COM_Z * Jw * Mb * q1 * q3 * rk * t4 * t6
+    * t9 * t18 * 36.0F;
+  f12 = COM_Y * Jw * Mb * q1 * q4 * rk * t3 * t6 * t11 * t18 * 24.0F;
+  f13 = COM_Y * Jw * Mb * q2 * q3 * rk * t2 * t6 * t12 * t18;
+  f14 = COM_Y * Jw * Mb * q2 * q3 * rk * t4 * t6 * t10 * t18;
+  f15 = COM_Z * Jw * Mb * q1 * q3 * rk * t2 * t6 * t12;
+  f16 = COM_Z * Jw * Mb * q1 * q3 * rk * t3 * t6 * t11;
+  f17 = COM_Z * Jw * Mb * q1 * q3 * rk * t4 * t6;
+  f18 = COM_Y * Jw * Mb * q1 * q4 * rk * t3 * t6;
+  f19 = COM_Z * Jw * Mb * q1 * q3 * rk * t3;
+  f20 = COM_Z * Jw * Mb * q2 * q4 * rk * t2;
+  f21 = COM_Z * Jw * Mb * q2 * q4 * rk;
+  f22 = COM_Y * Jw * Mb * q1 * q4 * rk;
+  f23 = COM_Z * Jw * Mb * q1 * q3;
+  f24 = COM_Z * Jw * Mb * q2 * q4;
+  f25 = COM_Z * Jw * Mb * q2;
+  f26 = COM_Z * Jw * Mb * q2;
+  f27 = COM_Y * Jw * Mb;
+  f28 = COM_Z * Jw * Mb;
+  f29 = COM_Z * Jw;
+  f30 = COM_Z * Jw;
+  f11 = ((((((((((((((((((((((((((((((((((((((((((((((((f11 - f12) + f13 * 24.0F)
+    + f14 * 24.0F) + f15 * t18 * 12.0F) + f16 * t18 * 12.0F) + f17 * t10 * t18 *
+    12.0F) + f18 * t12 * t18 * 72.0F) - f19 * t6 * t12 * t18 * 36.0F) - f20 * t6
+    * t11 * t18 * 36.0F) - f21 * t4 * t6 * t9 * t18 * 36.0F) - f22 * t6 * t7 *
+    t9 * t18 * 24.0F) + f23 * rk * t6 * t7 * t9 * t18 * 12.0F) + f24 * rk * t2 *
+    t6 * t12 * t18 * 12.0F) + f25 * q4 * rk * t3 * t6 * t11 * t18 * 12.0F) + f26
+    * q4 * rk * t4 * t6 * t10 * t18 * 12.0F) - f27 * q2 * q3 * rk * t6 * t7 *
+    t10 * t18 * 72.0F) - f28 * q1 * q3 * rk * t6 * t7 * t10 * t18 * 36.0F) - f29
+    * Mb * q2 * q4 * rk * t3 * t6 * t12 * t18 * 36.0F) + f30 * Mb * q2 * q4 * rk
+    * t6 * t7 * t9 * t18 * 12.0F) - COM_Z * Jw * Mb * q2 * q4 * rk * t6 * t7 *
+    t10 * t18 * 36.0F) - COM_Y * Jw * Mb * q1 * q4 * rk * t6 * t9 * t11 * t18 *
+    24.0F) + COM_Z * Jw * Mb * q1 * q3 * rk * t6 * t9 * t11 * t18 * 12.0F) +
+    COM_Y * Jw * Mb * q1 * q4 * rk * t6 * t9 * t12 * t18 * 24.0F) - COM_Y * Jw *
+    Mb * q2 * q3 * rk * t6 * t10 * t11 * t18 * 24.0F) - COM_Z * Jw * Mb * q1 *
+    q3 * rk * t6 * t9 * t12 * t18 * 12.0F) - COM_Z * Jw * Mb * q1 * q3 * rk * t6
+    * t10 * t11 * t18 * 12.0F) + COM_Y * Jw * Mb * q2 * q3 * rk * t6 * t10 * t12
+    * t18 * 24.0F) + COM_Z * Jw * Mb * q1 * q3 * rk * t6 * t10 * t12 * t18 *
+    12.0F) + COM_Z * Jw * Mb * q2 * q4 * rk * t6 * t9 * t11 * t18 * 12.0F) -
+    COM_Z * Jw * Mb * q2 * q4 * rk * t6 * t9 * t12 * t18 * 12.0F) - COM_Z * Jw *
+    Mb * q2 * q4 * rk * t6 * t10 * t11 * t18 * 12.0F) + COM_Z * Jw * Mb * q2 *
+    q4 * rk * t6 * t10 * t12 * t18 * 12.0F) + COM_X * COM_Y * g * q1 * q2 * rk *
+                        t2 * t3 * t19 * t20 * 32.0F) - COM_X * COM_Y * g * q3 *
+                       q4 * rk * t2 * t3 * t19 * t20 * 32.0F) - COM_Y * COM_Z *
+                      g * q1 * q4 * rk * t2 * t3 * t19 * t20 * 32.0F) - COM_Y *
+                     COM_Z * g * q2 * q3 * rk * t2 * t3 * t19 * t20 * 32.0F) -
+                    COM_X * COM_Y * g * q1 * q2 * rk * t2 * t9 * t19 * t20 *
+                    32.0F) + COM_X * COM_Y * g * q1 * q2 * rk * t2 * t10 * t19 *
+                   t20 * 32.0F) + COM_X * COM_Y * g * q1 * q2 * rk * t3 * t9 *
+                  t19 * t20 * 32.0F) + COM_X * COM_Y * g * q1 * q2 * rk * t3 *
+                 t10 * t19 * t20 * 96.0F) - COM_X * COM_Y * g * q3 * q4 * rk *
+                t2 * t9 * t19 * t20 * 96.0F) - COM_Y * COM_Z * g * q1 * q4 * rk *
+               t2 * t9 * t19 * t20 * 32.0F) - COM_Y * COM_Z * g * q2 * q3 * rk *
+              t2 * t9 * t19 * t20 * 32.0F) - COM_X * COM_Y * g * q3 * q4 * rk *
+             t2 * t10 * t19 * t20 * 32.0F) - COM_X * COM_Y * g * q3 * q4 * rk *
+            t3 * t9 * t19 * t20 * 32.0F) + COM_Y * COM_Z * g * q1 * q4 * rk * t2
+           * t10 * t19 * t20 * 32.0F) - COM_Y * COM_Z * g * q1 * q4 * rk * t3 *
+          t9 * t19 * t20 * 96.0F) - COM_Y * COM_Z * g * q2 * q3 * rk * t2 * t10 *
+         t19 * t20 * 96.0F) + COM_Y * COM_Z * g * q2 * q3 * rk * t3 * t9 * t19 *
+    t20 * 32.0F;
+  f12 = COM_X * COM_Y * g * q3 * q4 * rk * t3 * t10 * t19 * t20 * 32.0F;
+  f13 = COM_Y * COM_Z * g * q1 * q4 * rk * t3 * t10 * t19 * t20 * 32.0F;
+  f14 = COM_Y * COM_Z * g * q2 * q3 * rk * t3 * t10 * t19 * t20;
+  f15 = COM_X * COM_Y * g * q1 * q2 * rk * t9 * t10 * t19 * t20;
+  f16 = COM_X * COM_Y * g * q3 * q4 * rk * t9 * t10 * t19;
+  f17 = COM_Y * COM_Z * g * q1 * q4 * rk * t9 * t10 * t19;
+  f18 = COM_Y * COM_Z * g * q2 * q3 * rk * t9 * t10;
+  f19 = COM_X * Jw * Mb * dx * dy * q2 * q3 * rk * t2;
+  f20 = COM_Z * Jw * Mb * dx * dy * q1 * q2 * rk;
+  f21 = COM_X * Jw * Mb * dx * dy * q1 * q4 * rk;
+  f22 = COM_Z * Jw * Mb * dx * dy * q1 * q2;
+  f23 = COM_X * Jw * Mb * dx * dy * q2 * q3;
+  f24 = COM_Z * Jw * Mb * dx * dy * q1;
+  f25 = COM_X * Jw * Mb * dx * dy * q1;
+  f26 = COM_Z * Jw * Mb * dx * dy;
+  f27 = COM_Z * Jw * Mb * dx * dy;
+  f28 = COM_Z * Jw * Mb * dx;
+  f29 = COM_X * Jw * Mb * dx;
+  f30 = COM_X * Jw * Mb;
+  f31 = COM_Z * Jw * Mb;
+  t24 = COM_Z * Jw;
+  f32 = COM_Z * Jw;
+  f11 = (((((((((((((((((((((((((((((((((((((((((((((((((f11 + f12) - f13) - f14
+    * 32.0F) + f15 * 32.0F) - f16 * t20 * 32.0F) - f17 * t20 * 32.0F) - f18 *
+    t19 * t20 * 32.0F) - f19 * t4 * t18 * 24.0F) + f20 * t2 * t4 * t18 * 12.0F)
+    + f21 * t3 * t4 * t18 * 24.0F) - f22 * rk * t3 * t4 * t18 * 12.0F) + f23 *
+    rk * t2 * t7 * t18 * 24.0F) - f24 * q2 * rk * t2 * t7 * t18 * 12.0F) - f25 *
+    q4 * rk * t3 * t7 * t18 * 24.0F) + f26 * q1 * q2 * rk * t3 * t7 * t18 *
+    12.0F) - f27 * q3 * q4 * rk * t2 * t4 * t18 * 12.0F) + f28 * dy * q3 * q4 *
+    rk * t3 * t4 * t18 * 12.0F) + f29 * dy * q1 * q4 * rk * t4 * t9 * t18 *
+    72.0F) + f30 * dx * dy * q2 * q3 * rk * t2 * t11 * t18 * 72.0F) - f31 * dx *
+    dy * q1 * q2 * rk * t2 * t11 * t18 * 36.0F) - t24 * Mb * dx * dy * q1 * q2 *
+    rk * t4 * t9 * t18 * 36.0F) + f32 * Mb * dx * dy * q3 * q4 * rk * t2 * t7 *
+    t18 * 12.0F) - COM_X * Jw * Mb * dx * dy * q1 * q4 * rk * t3 * t11 * t18 *
+    24.0F) - COM_X * Jw * Mb * dx * dy * q2 * q3 * rk * t2 * t12 * t18 * 24.0F)
+    - COM_X * Jw * Mb * dx * dy * q2 * q3 * rk * t4 * t10 * t18 * 24.0F) + COM_Z
+    * Jw * Mb * dx * dy * q1 * q2 * rk * t2 * t12 * t18 * 12.0F) + COM_Z * Jw *
+    Mb * dx * dy * q1 * q2 * rk * t3 * t11 * t18 * 12.0F) + COM_Z * Jw * Mb * dx
+    * dy * q1 * q2 * rk * t4 * t10 * t18 * 12.0F) - COM_Z * Jw * Mb * dx * dy *
+    q3 * q4 * rk * t3 * t7 * t18 * 12.0F) + COM_X * Jw * Mb * dx * dy * q1 * q4 *
+    rk * t3 * t12 * t18 * 72.0F) - COM_Z * Jw * Mb * dx * dy * q1 * q2 * rk * t3
+    * t12 * t18 * 36.0F) - COM_X * Jw * Mb * dx * dy * q1 * q4 * rk * t7 * t9 *
+    t18 * 24.0F) + COM_Z * Jw * Mb * dx * dy * q1 * q2 * rk * t7 * t9 * t18 *
+    12.0F) + COM_X * Jw * Mb * dx * dy * q2 * q3 * rk * t7 * t10 * t18 * 72.0F)
+                       - COM_Z * Jw * Mb * dx * dy * q1 * q2 * rk * t7 * t10 *
+                       t18 * 36.0F) + COM_Z * Jw * Mb * dx * dy * q3 * q4 * rk *
+                      t2 * t11 * t18 * 36.0F) + COM_Z * Jw * Mb * dx * dy * q3 *
+                     q4 * rk * t4 * t9 * t18 * 36.0F) - COM_Z * Jw * Mb * dx *
+                    dy * q3 * q4 * rk * t2 * t12 * t18 * 12.0F) - COM_Z * Jw *
+                   Mb * dx * dy * q3 * q4 * rk * t3 * t11 * t18 * 12.0F) - COM_Z
+                  * Jw * Mb * dx * dy * q3 * q4 * rk * t4 * t10 * t18 * 12.0F) +
+                 COM_Z * Jw * Mb * dx * dy * q3 * q4 * rk * t3 * t12 * t18 *
+                 36.0F) - COM_X * Jw * Mb * dx * dy * q1 * q4 * rk * t9 * t11 *
+                t18 * 24.0F) + COM_Z * Jw * Mb * dx * dy * q1 * q2 * rk * t9 *
+               t11 * t18 * 12.0F) - COM_Z * Jw * Mb * dx * dy * q3 * q4 * rk *
+              t7 * t9 * t18 * 12.0F) + COM_X * Jw * Mb * dx * dy * q1 * q4 * rk *
+             t9 * t12 * t18 * 24.0F) + COM_X * Jw * Mb * dx * dy * q2 * q3 * rk *
+            t10 * t11 * t18 * 24.0F) - COM_Z * Jw * Mb * dx * dy * q1 * q2 * rk *
+           t9 * t12 * t18 * 12.0F) - COM_Z * Jw * Mb * dx * dy * q1 * q2 * rk *
+          t10 * t11 * t18 * 12.0F) + COM_Z * Jw * Mb * dx * dy * q3 * q4 * rk *
+         t7 * t10 * t18 * 36.0F) - COM_X * Jw * Mb * dx * dy * q2 * q3 * rk *
+    t10 * t12 * t18 * 24.0F;
+  f12 = COM_Z * Jw * Mb * dx * dy * q1 * q2 * rk * t10 * t12 * t18 * 12.0F;
+  f13 = COM_Z * Jw * Mb * dx * dy * q3 * q4 * rk * t9 * t11 * t18;
+  f14 = COM_Z * Jw * Mb * dx * dy * q3 * q4 * rk * t9 * t12 * t18;
+  f15 = COM_Z * Jw * Mb * dx * dy * q3 * q4 * rk * t10 * t11;
+  f16 = COM_Z * Jw * Mb * dx * dy * q3 * q4 * rk * t10 * t12;
+  f17 = COM_Y * Jw * Mb * g * q1 * q4 * t2 * t3 * t8;
+  f18 = COM_Y * Jw * Mb * g * q2 * q3 * t2 * t3 * t8;
+  f19 = COM_Z * Jw * Mb * g * q1 * q3 * t2 * t3;
+  f20 = COM_Y * Jw * Mb * g * q1 * q4 * t2 * t3;
+  f21 = COM_Y * Jw * Mb * g * q2 * q3 * t2;
+  f22 = COM_Z * Jw * Mb * g * q1 * q3 * t2;
+  f23 = COM_Z * Jw * Mb * g * q2 * q4;
+  f24 = COM_Z * Jw * Mb * g * q2 * q4;
+  f25 = COM_Y * Jw * Mb * g * q1;
+  f26 = COM_Y * Jw * Mb * g * q2;
+  f27 = COM_Z * Jw * Mb * g;
+  f28 = COM_Y * Jw * Mb * g;
+  f29 = COM_Y * Jw * Mb;
+  f30 = COM_Z * Jw * Mb;
+  f31 = COM_Z * Jw;
+  t24 = COM_Z * Jw;
+  acceleration[0] = rk * t476 * (((((((((((((((((((((((((((((((((((((((((((((f11
+    + f12) - f13 * 12.0F) + f14 * 12.0F) + f15 * t18 * 12.0F) - f16 * t18 *
+    12.0F) - f17 * t9 * t18 * 72.0F) + f18 * t9 * t18 * 120.0F) + f19 * t8 * t9 *
+    t18 * 168.0F) - f20 * t8 * t10 * t18 * 120.0F) + f21 * t3 * t8 * t10 * t18 *
+    72.0F) + f22 * t3 * t8 * t10 * t18 * 168.0F) + f23 * t2 * t3 * t8 * t9 * t18
+    * 168.0F) + f24 * t2 * t3 * t8 * t10 * t18 * 168.0F) - f25 * q4 * t2 * t8 *
+    t9 * t10 * t18 * 120.0F) + f26 * q3 * t2 * t8 * t9 * t10 * t18 * 72.0F) +
+    f27 * q1 * q3 * t2 * t8 * t9 * t10 * t18 * 168.0F) - f28 * q1 * q4 * t3 * t8
+    * t9 * t10 * t18 * 72.0F) + f29 * g * q2 * q3 * t3 * t8 * t9 * t10 * t18 *
+    120.0F) + f30 * g * q1 * q3 * t3 * t8 * t9 * t10 * t18 * 168.0F) + f31 * Mb *
+    g * q2 * q4 * t2 * t8 * t9 * t10 * t18 * 168.0F) + t24 * Mb * g * q2 * q4 *
+    t3 * t8 * t9 * t10 * t18 * 168.0F) + COM_Y * Jw * Mb * q2 * q3 * rk * t2 *
+    t3 * t6 * t9 * t18 * 48.0F) + COM_Z * Jw * Mb * q1 * q3 * rk * t2 * t3 * t6 *
+    t9 * t18 * 24.0F) - COM_Y * Jw * Mb * q1 * q4 * rk * t2 * t3 * t6 * t10 *
+    t18 * 48.0F) + COM_Z * Jw * Mb * q1 * q3 * rk * t2 * t3 * t6 * t10 * t18 *
+    24.0F) + COM_Z * Jw * Mb * q2 * q4 * rk * t2 * t3 * t6 * t9 * t18 * 24.0F) +
+    COM_Z * Jw * Mb * q2 * q4 * rk * t2 * t3 * t6 * t10 * t18 * 24.0F) - COM_Y *
+    Jw * Mb * q1 * q4 * rk * t2 * t6 * t9 * t10 * t18 * 48.0F) + COM_Z * Jw * Mb
+    * q1 * q3 * rk * t2 * t6 * t9 * t10 * t18 * 24.0F) + COM_Y * Jw * Mb * q2 *
+    q3 * rk * t3 * t6 * t9 * t10 * t18 * 48.0F) + COM_Z * Jw * Mb * q1 * q3 * rk
+    * t3 * t6 * t9 * t10 * t18 * 24.0F) + COM_Z * Jw * Mb * q2 * q4 * rk * t2 *
+    t6 * t9 * t10 * t18 * 24.0F) + COM_Z * Jw * Mb * q2 * q4 * rk * t3 * t6 * t9
+    * t10 * t18 * 24.0F) - COM_X * Jw * Mb * dx * dy * q2 * q3 * rk * t2 * t3 *
+    t9 * t18 * 48.0F) + COM_Z * Jw * Mb * dx * dy * q1 * q2 * rk * t2 * t3 * t9 *
+    t18 * 24.0F) - COM_X * Jw * Mb * dx * dy * q1 * q4 * rk * t2 * t3 * t10 *
+    t18 * 48.0F) + COM_Z * Jw * Mb * dx * dy * q1 * q2 * rk * t2 * t3 * t10 *
+    t18 * 24.0F) - COM_Z * Jw * Mb * dx * dy * q3 * q4 * rk * t2 * t3 * t9 * t18
+    * 24.0F) - COM_Z * Jw * Mb * dx * dy * q3 * q4 * rk * t2 * t3 * t10 * t18 *
+    24.0F) - COM_X * Jw * Mb * dx * dy * q1 * q4 * rk * t2 * t9 * t10 * t18 *
+    48.0F) + COM_Z * Jw * Mb * dx * dy * q1 * q2 * rk * t2 * t9 * t10 * t18 *
+    24.0F) - COM_X * Jw * Mb * dx * dy * q2 * q3 * rk * t3 * t9 * t10 * t18 *
+    48.0F) + COM_Z * Jw * Mb * dx * dy * q1 * q2 * rk * t3 * t9 * t10 * t18 *
+    24.0F) - COM_Z * Jw * Mb * dx * dy * q3 * q4 * rk * t2 * t9 * t10 * t18 *
+    24.0F) - COM_Z * Jw * Mb * dx * dy * q3 * q4 * rk * t3 * t9 * t10 * t18 *
+    24.0F) * -2.0F;
+  f11 = (((((((((((((((((((q1 * q2 * t5 * t8 * t13 * t17 * -9.0F + q1 * q2 * t5 *
+    t8 * t14 * t17 * 9.0F) + q1 * q2 * t5 * t8 * t15 * t17 * 9.0F) - q1 * q2 *
+    t5 * t8 * t16 * t17 * 9.0F) + q3 * q4 * t5 * t8 * t13 * t17 * 9.0F) - q3 *
+                       q4 * t5 * t8 * t14 * t17 * 9.0F) - q3 * q4 * t5 * t8 *
+                      t15 * t17 * 9.0F) + q3 * q4 * t5 * t8 * t16 * t17 * 9.0F)
+                    + COM_Y * Jk * Mb * g * t2 * t20 * 8.0F) + COM_Y * Jk * Mb *
+                   g * t3 * t20 * 8.0F) - COM_Y * Jk * Mb * g * t9 * t20 * 8.0F)
+                 - COM_Y * Jk * Mb * g * t10 * t20 * 8.0F) + COM_Y * g * t2 * t8
+                * t19 * t20 * 8.0F) + COM_Y * g * t3 * t8 * t19 * t20 * 8.0F) -
+              COM_Y * g * t8 * t9 * t19 * t20 * 8.0F) - COM_Y * g * t8 * t10 *
+             t19 * t20 * 8.0F) + COM_Y * Mb * Mk * g * t2 * t8 * t20 * 8.0F) +
+           COM_Y * Mb * Mk * g * t3 * t8 * t20 * 8.0F) - COM_Y * Mb * Mk * g *
+          t8 * t9 * t20 * 8.0F) - COM_Y * Mb * Mk * g * t8 * t10 * t20 * 8.0F) -
+    Jk * Jw * q1 * q2 * t2 * t17 * t18 * 12.0F;
+  f12 = Jk * Jw * q1 * q2 * t3 * t17 * t18 * 12.0F;
+  f13 = Jk * Jw * q3 * q4 * t2 * t17 * t18 * 12.0F;
+  f14 = Jk * Jw * q3 * q4 * t3 * t17 * t18;
+  f15 = Jk * Jw * q1 * q2 * t9 * t17 * t18;
+  f16 = Jk * Jw * q1 * q2 * t10 * t17;
+  f17 = Jk * Jw * q3 * q4 * t9 * t17;
+  f18 = Jk * Jw * q3 * q4 * t10;
+  f19 = COM_X * g * q1 * q4 * t8;
+  f20 = COM_X * g * q2 * q3;
+  f21 = COM_Z * g * q1 * q2;
+  f22 = COM_Z * g * q3;
+  f23 = dx * dy * q1;
+  f24 = dx * dy;
+  f25 = dx * dy;
+  f11 = (((((((((((((((((((((((((((((((((((((((((((((((((f11 + f12) + f13) - f14
+    * 12.0F) + f15 * 12.0F) - f16 * t18 * 12.0F) - f17 * t18 * 12.0F) + f18 *
+    t17 * t18 * 12.0F) + f19 * t19 * t20 * 16.0F) + f20 * t8 * t19 * t20 * 16.0F)
+    - f21 * t8 * t19 * t20 * 16.0F) + f22 * q4 * t8 * t19 * t20 * 16.0F) - f23 *
+    q3 * t5 * t8 * t13 * 9.0F) + f24 * q1 * q3 * t5 * t8 * t14 * 9.0F) + f25 *
+    q1 * q3 * t5 * t8 * t15 * 9.0F) - dx * dy * q2 * q4 * t5 * t8 * t13 * 9.0F)
+    - dx * dy * q1 * q3 * t5 * t8 * t16 * 9.0F) + dx * dy * q2 * q4 * t5 * t8 *
+    t14 * 9.0F) + dx * dy * q2 * q4 * t5 * t8 * t15 * 9.0F) - dx * dy * q2 * q4 *
+    t5 * t8 * t16 * 9.0F) + q1 * q2 * t2 * t4 * t5 * t6 * t8 * 36.0F) - q1 * q2 *
+    t3 * t5 * t6 * t7 * t8 * 36.0F) - q3 * q4 * t2 * t4 * t5 * t6 * t8 * 36.0F)
+    + q1 * q2 * t3 * t5 * t6 * t8 * t11 * 72.0F) - q1 * q2 * t4 * t5 * t6 * t8 *
+    t10 * 72.0F) + q3 * q4 * t3 * t5 * t6 * t7 * t8 * 36.0F) + q1 * q2 * t2 * t4
+    * t5 * t8 * t17 * 9.0F) + q1 * q2 * t3 * t4 * t5 * t8 * t17 * 9.0F) + q3 *
+    q4 * t2 * t5 * t6 * t8 * t12 * 72.0F) - q1 * q2 * t2 * t5 * t7 * t8 * t17 *
+    9.0F) - q1 * q2 * t5 * t6 * t8 * t9 * t11 * 36.0F) - q3 * q4 * t5 * t6 * t7 *
+    t8 * t9 * 72.0F) - q1 * q2 * t3 * t5 * t7 * t8 * t17 * 9.0F) - q3 * q4 * t2 *
+    t4 * t5 * t8 * t17 * 9.0F) + q1 * q2 * t5 * t6 * t8 * t10 * t12 * 36.0F) -
+                       q3 * q4 * t3 * t4 * t5 * t8 * t17 * 9.0F) - q1 * q2 * t2 *
+                      t5 * t8 * t11 * t17 * 27.0F) + q1 * q2 * t4 * t5 * t8 * t9
+                     * t17 * 27.0F) + q3 * q4 * t2 * t5 * t7 * t8 * t17 * 9.0F)
+                   + q3 * q4 * t5 * t6 * t8 * t9 * t11 * 36.0F) + q1 * q2 * t2 *
+                  t5 * t8 * t12 * t17 * 45.0F) - q1 * q2 * t3 * t5 * t8 * t11 *
+                 t17 * 117.0F) + q1 * q2 * t4 * t5 * t8 * t10 * t17 * 117.0F) +
+               q3 * q4 * t3 * t5 * t7 * t8 * t17 * 9.0F) + q1 * q2 * t3 * t5 *
+              t8 * t12 * t17 * 27.0F) - q3 * q4 * t5 * t6 * t8 * t10 * t12 *
+             36.0F) - q1 * q2 * t4 * t5 * t8 * t12 * t17 * 54.0F) - q1 * q2 * t5
+           * t7 * t8 * t9 * t17 * 45.0F) - q1 * q2 * t5 * t7 * t8 * t10 * t17 *
+          27.0F) + q3 * q4 * t2 * t5 * t8 * t11 * t17 * 27.0F) - q3 * q4 * t4 *
+    t5 * t8 * t9 * t17 * 27.0F;
+  f12 = q1 * q2 * t5 * t7 * t8 * t11 * t17 * 54.0F;
+  f13 = q3 * q4 * t2 * t5 * t8 * t12 * t17;
+  f14 = q3 * q4 * t3 * t5 * t8 * t11 * t17;
+  f15 = q3 * q4 * t4 * t5 * t8 * t10;
+  f16 = q3 * q4 * t3 * t5 * t8 * t12;
+  f17 = q1 * q2 * t5 * t8 * t9;
+  f18 = q3 * q4 * t4 * t5 * t8;
+  f19 = q3 * q4 * t5 * t7;
+  f20 = q1 * q2 * t5 * t8;
+  f21 = q1 * q2 * t5;
+  f22 = q3 * q4 * t5;
+  f23 = q1 * q2;
+  f24 = q3 * q4;
+  f11 = ((((((((((((((((((((((((((((((((((((((((((((((((f11 + f12) - f13 *
+    117.0F) + f14 * 45.0F) - f15 * t17 * 45.0F) - f16 * t17 * 27.0F) - f17 * t11
+    * t17 * 9.0F) + f18 * t12 * t17 * 54.0F) + f19 * t8 * t9 * t17 * 117.0F) +
+    f20 * t9 * t12 * t17 * 9.0F) - f21 * t8 * t10 * t11 * t17 * 9.0F) + f22 * t7
+    * t8 * t10 * t17 * 27.0F) + f23 * t5 * t8 * t10 * t12 * t17 * 9.0F) - f24 *
+    t5 * t7 * t8 * t11 * t17 * 54.0F) + q3 * q4 * t5 * t8 * t9 * t11 * t17 *
+    9.0F) - q3 * q4 * t5 * t8 * t9 * t12 * t17 * 9.0F) + q3 * q4 * t5 * t8 * t10
+    * t11 * t17 * 9.0F) - q3 * q4 * t5 * t8 * t10 * t12 * t17 * 9.0F) + COM_X *
+    Jk * Mb * g * q1 * q4 * t20 * 16.0F) + COM_X * Jk * Mb * g * q2 * q3 * t20 *
+    16.0F) - COM_Z * Jk * Mb * g * q1 * q2 * t20 * 16.0F) + COM_Z * Jk * Mb * g *
+    q3 * q4 * t20 * 16.0F) - COM_Y * Jw * Mb * g * t8 * t13 * t18 * 6.0F) -
+    COM_Y * Jw * Mb * g * t8 * t14 * t18 * 6.0F) + COM_Y * Jw * Mb * g * t8 *
+    t15 * t18 * 6.0F) + COM_Y * Jw * Mb * g * t8 * t16 * t18 * 6.0F) + COM_X *
+    Mb * Mk * g * q1 * q4 * t8 * t20 * 16.0F) + COM_X * Mb * Mk * g * q2 * q3 *
+    t8 * t20 * 16.0F) - COM_Z * Mb * Mk * g * q1 * q2 * t8 * t20 * 16.0F) +
+    COM_Z * Mb * Mk * g * q3 * q4 * t8 * t20 * 16.0F) + COM_Y * Jw * Mb * g * t2
+    * t4 * t8 * t18 * 18.0F) + COM_Y * Jw * Mb * g * t3 * t4 * t8 * t18 * 6.0F)
+    + COM_Y * Jw * Mb * g * t2 * t7 * t8 * t18 * 6.0F) + COM_Y * Jw * Mb * g *
+                        t3 * t7 * t8 * t18 * 18.0F) - COM_Y * Jw * Mb * g * t4 *
+                       t7 * t8 * t18 * 36.0F) - COM_Y * Jw * Mb * g * t2 * t8 *
+                      t11 * t18 * 6.0F) + COM_Y * Jw * Mb * g * t4 * t8 * t9 *
+                     t18 * 6.0F) - COM_Y * Jw * Mb * g * t2 * t8 * t12 * t18 *
+                    30.0F) - COM_Y * Jw * Mb * g * t3 * t8 * t11 * t18 * 30.0F)
+                  + COM_Y * Jw * Mb * g * t4 * t8 * t10 * t18 * 30.0F) - COM_Y *
+                 Jw * Mb * g * t3 * t8 * t12 * t18 * 6.0F) + COM_Y * Jw * Mb * g
+                * t7 * t8 * t9 * t18 * 30.0F) + COM_Y * Jw * Mb * g * t7 * t8 *
+               t10 * t18 * 6.0F) - COM_Y * Jw * Mb * g * t8 * t9 * t11 * t18 *
+              18.0F) - COM_Y * Jw * Mb * g * t8 * t9 * t12 * t18 * 6.0F) - COM_Y
+            * Jw * Mb * g * t8 * t10 * t11 * t18 * 6.0F) - COM_Y * Jw * Mb * g *
+           t8 * t10 * t12 * t18 * 18.0F) + COM_Y * Jw * Mb * g * t8 * t11 * t12 *
+          t18 * 36.0F) - Jk * Jw * dx * dy * q1 * q3 * t2 * t18 * 12.0F) + Jk *
+    Jw * dx * dy * q1 * q3 * t3 * t18 * 12.0F;
+  f12 = Jk * Jw * dx * dy * q2 * q4 * t2 * t18 * 12.0F;
+  f13 = Jk * Jw * dx * dy * q2 * q4 * t3 * t18 * 12.0F;
+  f14 = Jk * Jw * dx * dy * q1 * q3 * t9 * t18;
+  f15 = Jk * Jw * dx * dy * q1 * q3 * t10 * t18;
+  f16 = Jk * Jw * dx * dy * q2 * q4 * t9;
+  f17 = Jk * Jw * dx * dy * q2 * q4 * t10;
+  f18 = COM_Y * COM_Z * g * rk * t2 * t4;
+  f19 = COM_Y * COM_Z * g * rk * t3 * t4;
+  f20 = COM_Y * COM_Z * g * rk * t2;
+  f21 = COM_Y * COM_Z * g * rk * t3;
+  f22 = COM_Y * COM_Z * g * rk;
+  f23 = COM_Y * COM_Z * g * rk;
+  f24 = COM_Y * COM_Z * g;
+  f25 = COM_Y * COM_Z * g;
+  f26 = COM_Y * COM_Z;
+  f27 = COM_Y * COM_Z;
+  f11 = (((((((((((((((((((((((((((((((((((((((((((((((((f11 - f12) + f13) + f14
+    * 12.0F) - f15 * 12.0F) + f16 * t18 * 12.0F) - f17 * t18 * 12.0F) - f18 *
+    t19 * t20 * 8.0F) - f19 * t19 * t20 * 8.0F) + f20 * t7 * t19 * t20 * 8.0F) +
+    f21 * t7 * t19 * t20 * 8.0F) - f22 * t2 * t11 * t19 * t20 * 8.0F) - f23 * t4
+    * t9 * t19 * t20 * 8.0F) - f24 * rk * t2 * t12 * t19 * t20 * 40.0F) + f25 *
+    rk * t3 * t11 * t19 * t20 * 40.0F) + f26 * g * rk * t4 * t10 * t19 * t20 *
+    40.0F) + f27 * g * rk * t3 * t12 * t19 * t20 * 8.0F) - COM_Y * COM_Z * g *
+    rk * t7 * t9 * t19 * t20 * 40.0F) + COM_Y * COM_Z * g * rk * t7 * t10 * t19 *
+    t20 * 8.0F) - COM_Y * COM_Z * g * rk * t9 * t11 * t19 * t20 * 8.0F) + COM_Y *
+    COM_Z * g * rk * t9 * t12 * t19 * t20 * 8.0F) - COM_Y * COM_Z * g * rk * t10
+    * t11 * t19 * t20 * 8.0F) + COM_Y * COM_Z * g * rk * t10 * t12 * t19 * t20 *
+    8.0F) - Jw * Mb * q1 * q2 * t2 * t8 * t17 * t18 * 12.0F) + Jw * Mb * q1 * q2
+    * t3 * t8 * t17 * t18 * 12.0F) + Jw * Mb * q3 * q4 * t2 * t8 * t17 * t18 *
+    12.0F) - Jw * Mb * q3 * q4 * t3 * t8 * t17 * t18 * 12.0F) + Jw * Mb * q1 *
+    q2 * t8 * t9 * t17 * t18 * 12.0F) - Jw * Mb * q1 * q2 * t8 * t10 * t17 * t18
+    * 12.0F) - Jw * Mb * q3 * q4 * t8 * t9 * t17 * t18 * 12.0F) + Jw * Mb * q3 *
+    q4 * t8 * t10 * t17 * t18 * 12.0F) - Jw * Mk * q1 * q2 * t2 * t8 * t17 * t18
+    * 12.0F) + Jw * Mk * q1 * q2 * t3 * t8 * t17 * t18 * 12.0F) + Jw * Mk * q3 *
+    q4 * t2 * t8 * t17 * t18 * 12.0F) - Jw * Mk * q3 * q4 * t3 * t8 * t17 * t18 *
+                        12.0F) + Jw * Mk * q1 * q2 * t8 * t9 * t17 * t18 * 12.0F)
+                      - Jw * Mk * q1 * q2 * t8 * t10 * t17 * t18 * 12.0F) - Jw *
+                     Mk * q3 * q4 * t8 * t9 * t17 * t18 * 12.0F) + Jw * Mk * q3 *
+                    q4 * t8 * t10 * t17 * t18 * 12.0F) + dx * dy * q1 * q3 * t2 *
+                   t4 * t5 * t8 * 9.0F) + dx * dy * q1 * q3 * t3 * t4 * t5 * t8 *
+                  9.0F) - dx * dy * q2 * q4 * t2 * t4 * t5 * t8 * 135.0F) - dx *
+                dy * q1 * q3 * t2 * t5 * t7 * t8 * 9.0F) + dx * dy * q2 * q4 *
+               t3 * t4 * t5 * t8 * 9.0F) - dx * dy * q1 * q3 * t3 * t5 * t7 * t8
+              * 9.0F) - dx * dy * q2 * q4 * t2 * t5 * t7 * t8 * 9.0F) + dx * dy *
+            q2 * q4 * t3 * t5 * t7 * t8 * 135.0F) - dx * dy * q1 * q3 * t2 * t5 *
+           t8 * t11 * 27.0F) + dx * dy * q1 * q3 * t4 * t5 * t8 * t9 * 27.0F) +
+         dx * dy * q1 * q3 * t2 * t5 * t8 * t12 * 81.0F) - dx * dy * q1 * q3 *
+    t3 * t5 * t8 * t11 * 81.0F;
+  f12 = dx * dy * q1 * q3 * t4 * t5 * t8 * t10 * 81.0F;
+  f13 = dx * dy * q1 * q3 * t3 * t5 * t8 * t12;
+  f14 = dx * dy * q2 * q4 * t2 * t5 * t8 * t11;
+  f15 = dx * dy * q2 * q4 * t4 * t5 * t8;
+  f16 = dx * dy * q1 * q3 * t4 * t5 * t8;
+  f17 = dx * dy * q1 * q3 * t5 * t7;
+  f18 = dx * dy * q2 * q4 * t2 * t5;
+  f19 = dx * dy * q2 * q4 * t3;
+  f20 = dx * dy * q2 * q4 * t4;
+  f21 = dx * dy * q1 * q3;
+  f22 = dx * dy * q2 * q4;
+  f23 = dx * dy * q1;
+  f24 = dx * dy * q2;
+  f25 = dx * dy;
+  f26 = dx * dy;
+  f11 = ((((((((((((((((((((((((((((((((((((((((((((((((f11 + f12) + f13 * 27.0F)
+    - f14 * 27.0F) + f15 * t9 * 27.0F) - f16 * t12 * 54.0F) - f17 * t8 * t9 *
+    81.0F) + f18 * t8 * t12 * 81.0F) - f19 * t5 * t8 * t11 * 81.0F) + f20 * t5 *
+    t8 * t10 * 81.0F) - f21 * t5 * t7 * t8 * t10 * 27.0F) + f22 * t3 * t5 * t8 *
+    t12 * 27.0F) + f23 * q3 * t5 * t7 * t8 * t11 * 54.0F) - f24 * q4 * t4 * t5 *
+    t8 * t12 * 54.0F) - f25 * q2 * q4 * t5 * t7 * t8 * t9 * 81.0F) - f26 * q2 *
+    q4 * t5 * t7 * t8 * t10 * 27.0F) + dx * dy * q1 * q3 * t5 * t8 * t9 * t11 *
+    135.0F) + dx * dy * q2 * q4 * t5 * t7 * t8 * t11 * 54.0F) + dx * dy * q1 *
+    q3 * t5 * t8 * t9 * t12 * 9.0F) - dx * dy * q1 * q3 * t5 * t8 * t10 * t11 *
+    9.0F) - dx * dy * q1 * q3 * t5 * t8 * t10 * t12 * 135.0F) - dx * dy * q2 *
+    q4 * t5 * t8 * t9 * t11 * 9.0F) + dx * dy * q2 * q4 * t5 * t8 * t9 * t12 *
+    9.0F) - dx * dy * q2 * q4 * t5 * t8 * t10 * t11 * 9.0F) + dx * dy * q2 * q4 *
+    t5 * t8 * t10 * t12 * 9.0F) - g * q1 * q2 * rk * t4 * t19 * t20 * t21 *
+    32.0F) + g * q1 * q2 * rk * t4 * t19 * t20 * t22 * 16.0F) + g * q1 * q2 * rk
+    * t4 * t19 * t20 * t23 * 16.0F) - g * q1 * q2 * rk * t7 * t19 * t20 * t22 *
+    16.0F) + g * q1 * q2 * rk * t7 * t19 * t20 * t23 * 16.0F) - g * q3 * q4 * rk
+    * t4 * t19 * t20 * t22 * 16.0F) + g * q3 * q4 * rk * t4 * t19 * t20 * t23 *
+    16.0F) + g * q1 * q2 * rk * t11 * t19 * t20 * t21 * 32.0F) - g * q3 * q4 *
+                        rk * t7 * t19 * t20 * t21 * 32.0F) - g * q1 * q2 * rk *
+                       t11 * t19 * t20 * t22 * 16.0F) + g * q3 * q4 * rk * t7 *
+                      t19 * t20 * t22 * 16.0F) - g * q1 * q2 * rk * t11 * t19 *
+                     t20 * t23 * 16.0F) + g * q1 * q2 * rk * t12 * t19 * t20 *
+                    t22 * 16.0F) + g * q3 * q4 * rk * t7 * t19 * t20 * t23 *
+                   16.0F) - g * q1 * q2 * rk * t12 * t19 * t20 * t23 * 16.0F) +
+                 g * q3 * q4 * rk * t11 * t19 * t20 * t22 * 16.0F) + g * q3 * q4
+                * rk * t12 * t19 * t20 * t21 * 32.0F) - g * q3 * q4 * rk * t11 *
+               t19 * t20 * t23 * 16.0F) - g * q3 * q4 * rk * t12 * t19 * t20 *
+              t22 * 16.0F) - g * q3 * q4 * rk * t12 * t19 * t20 * t23 * 16.0F) -
+            q1 * q2 * t2 * t3 * t4 * t5 * t6 * t8 * 36.0F) + q1 * q2 * t2 * t3 *
+           t5 * t6 * t7 * t8 * 36.0F) + q3 * q4 * t2 * t3 * t4 * t5 * t6 * t8 *
+          36.0F) + q1 * q2 * t2 * t3 * t5 * t6 * t8 * t9 * 36.0F) + q1 * q2 * t2
+    * t3 * t5 * t6 * t8 * t10 * 36.0F;
+  f12 = q1 * q2 * t2 * t4 * t5 * t6 * t8 * t9 * 72.0F;
+  f13 = q1 * q2 * t2 * t3 * t5 * t6 * t8 * t11 * 108.0F;
+  f14 = q3 * q4 * t2 * t3 * t5 * t6 * t7 * t8;
+  f15 = q1 * q2 * t2 * t3 * t5 * t6 * t8 * t12;
+  f16 = q1 * q2 * t3 * t4 * t5 * t6 * t8;
+  f17 = q3 * q4 * t2 * t3 * t5 * t6 * t8;
+  f18 = q3 * q4 * t2 * t3 * t5 * t6;
+  f19 = q1 * q2 * t2 * t3 * t4 * t5;
+  f20 = q3 * q4 * t2 * t3 * t5;
+  f21 = q1 * q2 * t2 * t5 * t6;
+  f22 = q3 * q4 * t2 * t3;
+  f23 = q1 * q2 * t2 * t5;
+  f24 = q1 * q2 * t3;
+  f25 = q3 * q4 * t2;
+  f26 = q1 * q2;
+  f27 = q1 * q2;
+  f11 = (((((((((((((((((((((((((((((((((((((((((((((((((f11 - f12) - f13) - f14
+    * 36.0F) - f15 * 36.0F) + f16 * t10 * 72.0F) + f17 * t9 * 36.0F) + f18 * t8 *
+    t10 * 36.0F) - f19 * t8 * t17 * 18.0F) - f20 * t6 * t8 * t11 * 36.0F) - f21 *
+    t8 * t9 * t10 * 36.0F) - f22 * t5 * t6 * t8 * t12 * 108.0F) + f23 * t6 * t8 *
+    t9 * t11 * 72.0F) - f24 * t5 * t6 * t8 * t9 * t10 * 36.0F) + f25 * t5 * t6 *
+    t7 * t8 * t9 * 72.0F) + f26 * t2 * t3 * t5 * t7 * t8 * t17 * 18.0F) + f27 *
+    t4 * t5 * t6 * t8 * t9 * t10 * 108.0F) - q1 * q2 * t3 * t5 * t6 * t8 * t10 *
+    t11 * 72.0F) + q3 * q4 * t2 * t3 * t4 * t5 * t8 * t17 * 18.0F) - q3 * q4 *
+    t3 * t5 * t6 * t7 * t8 * t10 * 72.0F) + q1 * q2 * t2 * t3 * t5 * t8 * t9 *
+    t17 * 54.0F) - q3 * q4 * t2 * t5 * t6 * t8 * t9 * t10 * 36.0F) - q1 * q2 *
+    t2 * t3 * t5 * t8 * t10 * t17 * 126.0F) - q1 * q2 * t2 * t4 * t5 * t8 * t9 *
+    t17 * 90.0F) + q1 * q2 * t5 * t6 * t7 * t8 * t9 * t10 * 36.0F) - q3 * q4 *
+    t3 * t5 * t6 * t8 * t9 * t10 * 36.0F) + q1 * q2 * t2 * t3 * t5 * t8 * t11 *
+    t17 * 270.0F) - q1 * q2 * t3 * t4 * t5 * t8 * t9 * t17 * 36.0F) - q3 * q4 *
+    t2 * t3 * t5 * t7 * t8 * t17 * 18.0F) - q3 * q4 * t2 * t5 * t6 * t8 * t9 *
+    t12 * 72.0F) + q3 * q4 * t4 * t5 * t6 * t8 * t9 * t10 * 36.0F) - q1 * q2 *
+    t2 * t3 * t5 * t8 * t12 * t17 * 126.0F) - q1 * q2 * t3 * t4 * t5 * t8 * t10 *
+    t17 * 126.0F) + q1 * q2 * t2 * t5 * t7 * t8 * t9 * t17 * 54.0F) - q3 * q4 *
+                        t2 * t3 * t5 * t8 * t9 * t17 * 126.0F) + q3 * q4 * t3 *
+                       t5 * t6 * t8 * t10 * t12 * 72.0F) + q1 * q2 * t2 * t5 *
+                      t7 * t8 * t10 * t17 * 36.0F) + q1 * q2 * t5 * t6 * t8 * t9
+                     * t10 * t11 * 36.0F) + q3 * q4 * t2 * t3 * t5 * t8 * t10 *
+                    t17 * 54.0F) + q3 * q4 * t2 * t4 * t5 * t8 * t9 * t17 *
+                   162.0F) + q3 * q4 * t5 * t6 * t7 * t8 * t9 * t10 * 108.0F) +
+                 q1 * q2 * t3 * t5 * t7 * t8 * t10 * t17 * 162.0F) - q1 * q2 *
+                t5 * t6 * t8 * t9 * t10 * t12 * 36.0F) - q3 * q4 * t2 * t3 * t5 *
+               t8 * t11 * t17 * 126.0F) + q3 * q4 * t3 * t4 * t5 * t8 * t9 * t17
+              * 36.0F) - q1 * q2 * t2 * t5 * t8 * t9 * t10 * t17 * 54.0F) + q3 *
+            q4 * t2 * t3 * t5 * t8 * t12 * t17 * 270.0F) + q3 * q4 * t3 * t4 *
+           t5 * t8 * t10 * t17 * 54.0F) + q1 * q2 * t2 * t5 * t8 * t9 * t11 *
+          t17 * 90.0F) + q1 * q2 * t3 * t5 * t8 * t9 * t10 * t17 * 126.0F) - q3 *
+    q4 * t2 * t5 * t7 * t8 * t9 * t17 * 126.0F;
+  f12 = q1 * q2 * t2 * t5 * t8 * t9 * t12 * t17 * 54.0F;
+  f13 = q1 * q2 * t2 * t5 * t8 * t10 * t11 * t17;
+  f14 = q1 * q2 * t4 * t5 * t8 * t9 * t10 * t17;
+  f15 = q3 * q4 * t2 * t5 * t7 * t8 * t10;
+  f16 = q3 * q4 * t5 * t6 * t8 * t9 * t10;
+  f17 = q1 * q2 * t3 * t5 * t8 * t9;
+  f18 = q1 * q2 * t3 * t5 * t8 * t10;
+  f19 = q3 * q4 * t3 * t5 * t7;
+  f20 = q3 * q4 * t5 * t6 * t8;
+  f21 = q1 * q2 * t3 * t5;
+  f22 = q3 * q4 * t2 * t5;
+  f23 = q1 * q2 * t5;
+  f24 = q3 * q4 * t2;
+  f25 = q3 * q4;
+  f26 = q3 * q4;
+  f11 = ((((((((((((((((((((((((((((((((((((((((((((((((f11 - f12) + f13 * 36.0F)
+    - f14 * 270.0F) - f15 * t17 * 36.0F) - f16 * t11 * 36.0F) - f17 * t12 * t17 *
+    36.0F) + f18 * t11 * t17 * 126.0F) - f19 * t8 * t10 * t17 * 90.0F) + f20 *
+    t9 * t10 * t12 * 36.0F) - f21 * t8 * t10 * t12 * t17 * 162.0F) + f22 * t8 *
+    t9 * t10 * t17 * 126.0F) + f23 * t7 * t8 * t9 * t10 * t17 * 126.0F) - f24 *
+    t5 * t8 * t9 * t11 * t17 * 162.0F) - f25 * t3 * t5 * t8 * t9 * t10 * t17 *
+    54.0F) + f26 * t2 * t5 * t8 * t9 * t12 * t17 * 126.0F) - q3 * q4 * t2 * t5 *
+    t8 * t10 * t11 * t17 * 36.0F) + q3 * q4 * t4 * t5 * t8 * t9 * t10 * t17 *
+    126.0F) + q3 * q4 * t3 * t5 * t8 * t9 * t12 * t17 * 36.0F) - q3 * q4 * t3 *
+    t5 * t8 * t10 * t11 * t17 * 54.0F) + q3 * q4 * t3 * t5 * t8 * t10 * t12 *
+    t17 * 90.0F) + q1 * q2 * t5 * t8 * t9 * t10 * t11 * t17 * 18.0F) - q3 * q4 *
+    t5 * t7 * t8 * t9 * t10 * t17 * 270.0F) - q1 * q2 * t5 * t8 * t9 * t10 * t12
+    * t17 * 18.0F) - q3 * q4 * t5 * t8 * t9 * t10 * t11 * t17 * 18.0F) + q3 * q4
+    * t5 * t8 * t9 * t10 * t12 * t17 * 18.0F) - COM_Y * COM_Z * g * rk * t2 * t3
+    * t9 * t19 * t20 * 48.0F) + COM_Y * COM_Z * g * rk * t2 * t3 * t10 * t19 *
+    t20 * 48.0F) - COM_Y * COM_Z * g * rk * t2 * t9 * t10 * t19 * t20 * 48.0F) +
+    COM_Y * COM_Z * g * rk * t3 * t9 * t10 * t19 * t20 * 48.0F) - dx * dy * q1 *
+    q3 * t2 * t3 * t4 * t5 * t8 * 18.0F) + dx * dy * q2 * q4 * t2 * t3 * t4 * t5
+    * t8 * 126.0F) + dx * dy * q1 * q3 * t2 * t3 * t5 * t7 * t8 * 18.0F) + dx *
+                        dy * q1 * q3 * t2 * t3 * t5 * t8 * t9 * 90.0F) - dx * dy
+                       * q2 * q4 * t2 * t3 * t5 * t7 * t8 * 126.0F) - dx * dy *
+                      q1 * q3 * t2 * t3 * t5 * t8 * t10 * 90.0F) - dx * dy * q1 *
+                     q3 * t2 * t4 * t5 * t8 * t9 * 126.0F) + dx * dy * q1 * q3 *
+                    t2 * t3 * t5 * t8 * t11 * 198.0F) - dx * dy * q1 * q3 * t3 *
+                   t4 * t5 * t8 * t9 * 36.0F) - dx * dy * q2 * q4 * t2 * t3 * t5
+                  * t8 * t9 * 54.0F) - dx * dy * q1 * q3 * t2 * t3 * t5 * t8 *
+                 t12 * 198.0F) - dx * dy * q1 * q3 * t3 * t4 * t5 * t8 * t10 *
+                90.0F) + dx * dy * q2 * q4 * t2 * t3 * t5 * t8 * t10 * 54.0F) +
+              dx * dy * q2 * q4 * t2 * t4 * t5 * t8 * t9 * 162.0F) + dx * dy *
+             q1 * q3 * t2 * t5 * t7 * t8 * t9 * 90.0F) + dx * dy * q2 * q4 * t2 *
+            t3 * t5 * t8 * t11 * 54.0F) - dx * dy * q2 * q4 * t3 * t4 * t5 * t8 *
+           t9 * 36.0F) + dx * dy * q1 * q3 * t2 * t5 * t7 * t8 * t10 * 36.0F) -
+         dx * dy * q2 * q4 * t2 * t3 * t5 * t8 * t12 * 54.0F) - dx * dy * q2 *
+    q4 * t3 * t4 * t5 * t8 * t10 * 90.0F;
+  f12 = dx * dy * q1 * q3 * t3 * t5 * t7 * t8 * t10 * 126.0F;
+  f13 = dx * dy * q2 * q4 * t2 * t5 * t7 * t8 * t9 * 90.0F;
+  f14 = dx * dy * q1 * q3 * t2 * t5 * t8 * t9 * t10;
+  f15 = dx * dy * q2 * q4 * t2 * t5 * t7 * t8 * t10;
+  f16 = dx * dy * q1 * q3 * t2 * t5 * t8 * t9;
+  f17 = dx * dy * q1 * q3 * t3 * t5 * t8 * t9;
+  f18 = dx * dy * q2 * q4 * t3 * t5 * t7;
+  f19 = dx * dy * q1 * q3 * t2 * t5 * t8;
+  f20 = dx * dy * q1 * q3 * t2 * t5;
+  f21 = dx * dy * q1 * q3 * t4 * t5;
+  f22 = dx * dy * q2 * q4 * t2;
+  f23 = dx * dy * q1 * q3 * t3;
+  f24 = dx * dy * q1 * q3;
+  f25 = dx * dy * q2 * q4;
+  f26 = dx * dy * q2;
+  f27 = dx * dy * q1;
+  f28 = dx * dy;
+  f29 = dx * dy;
+  f11 = (((((((((((((((((((((((((((((((((((((((((((((((((f11 + f12) + f13) + f14
+    * 54.0F) + f15 * 36.0F) - f16 * t11 * 162.0F) - f17 * t10 * 54.0F) - f18 *
+    t8 * t10 * 162.0F) - f19 * t9 * t12 * 90.0F) + f20 * t8 * t10 * t11 * 36.0F)
+    - f21 * t8 * t9 * t10 * 54.0F) - f22 * t5 * t8 * t9 * t10 * 90.0F) - f23 *
+    t5 * t8 * t9 * t12 * 36.0F) + f24 * t3 * t5 * t8 * t10 * t11 * 90.0F) + f25 *
+    t2 * t5 * t8 * t9 * t11 * 126.0F) + f26 * q4 * t3 * t5 * t8 * t9 * t10 *
+    90.0F) + f27 * q3 * t3 * t5 * t8 * t10 * t12 * 162.0F) - f28 * q2 * q4 * t2 *
+    t5 * t8 * t9 * t12 * 90.0F) + f29 * q2 * q4 * t2 * t5 * t8 * t10 * t11 *
+    36.0F) - dx * dy * q2 * q4 * t4 * t5 * t8 * t9 * t10 * 198.0F) + dx * dy *
+    q1 * q3 * t5 * t7 * t8 * t9 * t10 * 54.0F) - dx * dy * q2 * q4 * t3 * t5 *
+    t8 * t9 * t12 * 36.0F) + dx * dy * q2 * q4 * t3 * t5 * t8 * t10 * t11 *
+    90.0F) - dx * dy * q2 * q4 * t3 * t5 * t8 * t10 * t12 * 126.0F) + dx * dy *
+    q2 * q4 * t5 * t7 * t8 * t9 * t10 * 198.0F) - dx * dy * q1 * q3 * t5 * t8 *
+    t9 * t10 * t11 * 126.0F) + dx * dy * q1 * q3 * t5 * t8 * t9 * t10 * t12 *
+    126.0F) + dx * dy * q2 * q4 * t5 * t8 * t9 * t10 * t11 * 18.0F) - dx * dy *
+    q2 * q4 * t5 * t8 * t9 * t10 * t12 * 18.0F) - g * q1 * q2 * rk * t2 * t3 *
+    t19 * t20 * t21 * 32.0F) + g * q1 * q2 * rk * t2 * t3 * t19 * t20 * t23 *
+    32.0F) - g * q3 * q4 * rk * t2 * t3 * t19 * t20 * t21 * 32.0F) + g * q3 * q4
+    * rk * t2 * t3 * t19 * t20 * t23 * 32.0F) + g * q1 * q2 * rk * t2 * t10 *
+    t19 * t20 * t21 * 32.0F) - g * q1 * q2 * rk * t3 * t9 * t19 * t20 * t21 *
+                        32.0F) - g * q1 * q2 * rk * t2 * t10 * t19 * t20 * t22 *
+                       32.0F) + g * q1 * q2 * rk * t3 * t9 * t19 * t20 * t22 *
+                      32.0F) - g * q3 * q4 * rk * t2 * t10 * t19 * t20 * t21 *
+                     32.0F) + g * q3 * q4 * rk * t3 * t9 * t19 * t20 * t21 *
+                    32.0F) + g * q3 * q4 * rk * t2 * t10 * t19 * t20 * t22 *
+                   32.0F) - g * q3 * q4 * rk * t3 * t9 * t19 * t20 * t22 * 32.0F)
+                 + g * q1 * q2 * rk * t9 * t10 * t19 * t20 * t21 * 32.0F) - g *
+                q1 * q2 * rk * t9 * t10 * t19 * t20 * t23 * 32.0F) + g * q3 * q4
+               * rk * t9 * t10 * t19 * t20 * t21 * 32.0F) - g * q3 * q4 * rk *
+              t9 * t10 * t19 * t20 * t23 * 32.0F) - COM_X * Jw * Mb * g * q1 *
+             q4 * t4 * t8 * t18 * 12.0F) + COM_X * Jw * Mb * g * q2 * q3 * t4 *
+            t8 * t18 * 12.0F) - COM_Z * Jw * Mb * g * q1 * q2 * t4 * t8 * t18 *
+           12.0F) + COM_X * Jw * Mb * g * q1 * q4 * t7 * t8 * t18 * 12.0F) -
+         COM_X * Jw * Mb * g * q2 * q3 * t7 * t8 * t18 * 12.0F) - COM_Z * Jw *
+    Mb * g * q1 * q2 * t7 * t8 * t18 * 12.0F;
+  f12 = COM_Z * Jw * Mb * g * q3 * q4 * t4 * t8 * t18 * 12.0F;
+  f13 = COM_X * Jw * Mb * g * q1 * q4 * t8 * t11 * t18;
+  f14 = COM_X * Jw * Mb * g * q2 * q3 * t8 * t11 * t18;
+  f15 = COM_Z * Jw * Mb * g * q1 * q2 * t8 * t11;
+  f16 = COM_Z * Jw * Mb * g * q3 * q4 * t7 * t8;
+  f17 = COM_X * Jw * Mb * g * q1 * q4 * t8;
+  f18 = COM_X * Jw * Mb * g * q2 * q3 * t8;
+  f19 = COM_Z * Jw * Mb * g * q1 * q2;
+  f20 = COM_Z * Jw * Mb * g * q3 * q4;
+  f21 = COM_Z * Jw * Mb * g * q3;
+  f22 = COM_Y * Jw * Mb * g * t2;
+  f23 = COM_Y * Jw * Mb * g;
+  f24 = COM_Y * Jw * Mb * g;
+  f25 = COM_Y * Jw * Mb;
+  f26 = COM_Y * Jw * Mb;
+  f27 = COM_Y * Jw;
+  f28 = COM_Y * Jw;
+  f11 = ((((((((((((((((((((((((((((((((((((((((((((((((f11 + f12) + f13 * 12.0F)
+    - f14 * 12.0F) - f15 * t18 * 12.0F) + f16 * t18 * 12.0F) - f17 * t12 * t18 *
+    12.0F) + f18 * t12 * t18 * 12.0F) - f19 * t8 * t12 * t18 * 12.0F) + f20 * t8
+    * t11 * t18 * 12.0F) + f21 * q4 * t8 * t12 * t18 * 12.0F) - f22 * t3 * t4 *
+    t8 * t18 * 24.0F) - f23 * t2 * t3 * t7 * t8 * t18 * 24.0F) - f24 * t2 * t3 *
+    t8 * t9 * t18 * 12.0F) - f25 * g * t2 * t3 * t8 * t10 * t18 * 12.0F) - f26 *
+    g * t2 * t4 * t8 * t9 * t18 * 12.0F) + f27 * Mb * g * t2 * t3 * t8 * t11 *
+    t18 * 48.0F) - f28 * Mb * g * t2 * t4 * t8 * t10 * t18 * 36.0F) - COM_Y * Jw
+    * Mb * g * t3 * t4 * t8 * t9 * t18 * 12.0F) + COM_Y * Jw * Mb * g * t2 * t3 *
+    t8 * t12 * t18 * 48.0F) - COM_Y * Jw * Mb * g * t3 * t4 * t8 * t10 * t18 *
+    36.0F) - COM_Y * Jw * Mb * g * t2 * t7 * t8 * t9 * t18 * 36.0F) - COM_Y * Jw
+    * Mb * g * t2 * t7 * t8 * t10 * t18 * 12.0F) - COM_Y * Jw * Mb * g * t3 * t7
+    * t8 * t9 * t18 * 36.0F) - COM_Y * Jw * Mb * g * t3 * t7 * t8 * t10 * t18 *
+    12.0F) + COM_Y * Jw * Mb * g * t2 * t8 * t9 * t10 * t18 * 12.0F) + COM_Y *
+    Jw * Mb * g * t2 * t8 * t9 * t11 * t18 * 12.0F) + COM_Y * Jw * Mb * g * t3 *
+    t8 * t9 * t10 * t18 * 12.0F) + COM_Y * Jw * Mb * g * t2 * t8 * t9 * t12 *
+    t18 * 36.0F) + COM_Y * Jw * Mb * g * t2 * t8 * t10 * t11 * t18 * 12.0F) +
+    COM_Y * Jw * Mb * g * t3 * t8 * t9 * t11 * t18 * 36.0F) - COM_Y * Jw * Mb *
+    g * t4 * t8 * t9 * t10 * t18 * 48.0F) + COM_Y * Jw * Mb * g * t2 * t8 * t10 *
+    t12 * t18 * 36.0F) + COM_Y * Jw * Mb * g * t3 * t8 * t9 * t12 * t18 * 12.0F)
+                       + COM_Y * Jw * Mb * g * t3 * t8 * t10 * t11 * t18 * 36.0F)
+                      + COM_Y * Jw * Mb * g * t3 * t8 * t10 * t12 * t18 * 12.0F)
+                     - COM_Y * Jw * Mb * g * t7 * t8 * t9 * t10 * t18 * 48.0F) +
+                    COM_Y * Jw * Mb * g * t8 * t9 * t10 * t11 * t18 * 24.0F) +
+                   COM_Y * Jw * Mb * g * t8 * t9 * t10 * t12 * t18 * 24.0F) +
+                  COM_Y * Jw * Mb * rk * t2 * t4 * t9 * t17 * t18 * 24.0F) +
+                 COM_Y * Jw * Mb * rk * t2 * t3 * t11 * t17 * t18 * 48.0F) +
+                COM_Y * Jw * Mb * rk * t2 * t3 * t12 * t17 * t18 * 48.0F) -
+               COM_Y * Jw * Mb * rk * t3 * t4 * t10 * t17 * t18 * 24.0F) - COM_Y
+              * Jw * Mb * rk * t2 * t7 * t9 * t17 * t18 * 24.0F) + COM_Y * Jw *
+             Mb * rk * t3 * t7 * t10 * t17 * t18 * 24.0F) - COM_Y * Jw * Mb * rk
+            * t2 * t9 * t11 * t17 * t18 * 24.0F) + COM_Y * Jw * Mb * rk * t2 *
+           t9 * t12 * t17 * t18 * 24.0F) - COM_Y * Jw * Mb * rk * t4 * t9 * t10 *
+          t17 * t18 * 48.0F) + COM_Y * Jw * Mb * rk * t3 * t10 * t11 * t17 * t18
+         * 24.0F) - COM_Y * Jw * Mb * rk * t3 * t10 * t12 * t17 * t18 * 24.0F;
+  f12 = COM_Y * Jw * Mb * rk * t7 * t9 * t10 * t17 * t18 * 48.0F;
+  f13 = COM_X * COM_Y * g * q1 * q3 * rk * t4 * t19 * t20 * 16.0F;
+  f14 = COM_X * COM_Y * g * q2 * q4 * rk * t4 * t19 * t20;
+  f15 = COM_X * COM_Z * g * q1 * q4 * rk * t4 * t19 * t20;
+  f16 = COM_X * COM_Z * g * q2 * q3 * rk * t4 * t19;
+  f17 = COM_X * COM_Y * g * q1 * q3 * rk * t7 * t19;
+  f18 = COM_X * COM_Y * g * q2 * q4 * rk * t7;
+  f19 = COM_X * COM_Z * g * q1 * q4 * rk * t7;
+  f20 = COM_X * COM_Z * g * q2 * q3 * rk;
+  f21 = COM_X * COM_Y * g * q1 * q3 * rk;
+  f22 = COM_X * COM_Y * g * q1 * q3;
+  f23 = COM_X * COM_Y * g * q2 * q4;
+  f24 = COM_X * COM_Z * g * q1;
+  f25 = COM_X * COM_Z * g * q2;
+  f26 = COM_X * COM_Y * g;
+  f27 = COM_X * COM_Z * g;
+  f28 = COM_X * COM_Z;
+  f29 = Jw * Mb;
+  f11 = (((((((((((((((((((((((((((((((((((((((((((((((((f11 - f12) - f13) + f14
+    * 48.0F) - f15 * 48.0F) - f16 * t20 * 16.0F) - f17 * t20 * 16.0F) + f18 *
+    t19 * t20 * 48.0F) + f19 * t19 * t20 * 16.0F) + f20 * t7 * t19 * t20 * 48.0F)
+    + f21 * t11 * t19 * t20 * 48.0F) + f22 * rk * t12 * t19 * t20 * 48.0F) - f23
+    * rk * t11 * t19 * t20 * 16.0F) + f24 * q4 * rk * t11 * t19 * t20 * 16.0F) +
+    f25 * q3 * rk * t11 * t19 * t20 * 48.0F) - f26 * q2 * q4 * rk * t12 * t19 *
+    t20 * 16.0F) - f27 * q1 * q4 * rk * t12 * t19 * t20 * 48.0F) - f28 * g * q2 *
+    q3 * rk * t12 * t19 * t20 * 16.0F) - f29 * dx * dy * q1 * q3 * t2 * t8 * t18
+    * 12.0F) + Jw * Mb * dx * dy * q1 * q3 * t3 * t8 * t18 * 12.0F) - Jw * Mb *
+    dx * dy * q2 * q4 * t2 * t8 * t18 * 12.0F) + Jw * Mb * dx * dy * q2 * q4 *
+    t3 * t8 * t18 * 12.0F) + Jw * Mb * dx * dy * q1 * q3 * t8 * t9 * t18 * 12.0F)
+    - Jw * Mb * dx * dy * q1 * q3 * t8 * t10 * t18 * 12.0F) + Jw * Mb * dx * dy *
+    q2 * q4 * t8 * t9 * t18 * 12.0F) - Jw * Mb * dx * dy * q2 * q4 * t8 * t10 *
+    t18 * 12.0F) - Jw * Mk * dx * dy * q1 * q3 * t2 * t8 * t18 * 12.0F) + Jw *
+    Mk * dx * dy * q1 * q3 * t3 * t8 * t18 * 12.0F) - Jw * Mk * dx * dy * q2 *
+    q4 * t2 * t8 * t18 * 12.0F) + Jw * Mk * dx * dy * q2 * q4 * t3 * t8 * t18 *
+    12.0F) + Jw * Mk * dx * dy * q1 * q3 * t8 * t9 * t18 * 12.0F) - Jw * Mk * dx
+    * dy * q1 * q3 * t8 * t10 * t18 * 12.0F) + Jw * Mk * dx * dy * q2 * q4 * t8 *
+    t9 * t18 * 12.0F) - Jw * Mk * dx * dy * q2 * q4 * t8 * t10 * t18 * 12.0F) -
+                        COM_X * Jw * Mb * dx * dy * rk * t2 * t3 * t4 * t18 *
+                        24.0F) + COM_X * Jw * Mb * dx * dy * rk * t2 * t3 * t7 *
+                       t18 * 24.0F) - COM_X * Jw * Mb * dx * dy * rk * t2 * t4 *
+                      t9 * t18 * 48.0F) + COM_X * Jw * Mb * dx * dy * rk * t2 *
+                     t3 * t11 * t18 * 24.0F) - COM_X * Jw * Mb * dx * dy * rk *
+                    t2 * t3 * t12 * t18 * 24.0F) + COM_X * Jw * Mb * dx * dy *
+                   rk * t3 * t7 * t10 * t18 * 48.0F) - COM_X * Jw * Mb * dx * dy
+                  * rk * t2 * t9 * t11 * t18 * 48.0F) + COM_X * Jw * Mb * dx *
+                 dy * rk * t4 * t9 * t10 * t18 * 24.0F) + COM_X * Jw * Mb * dx *
+                dy * rk * t3 * t10 * t12 * t18 * 48.0F) - COM_X * Jw * Mb * dx *
+               dy * rk * t7 * t9 * t10 * t18 * 24.0F) - COM_X * Jw * Mb * dx *
+              dy * rk * t9 * t10 * t11 * t18 * 24.0F) + COM_X * Jw * Mb * dx *
+             dy * rk * t9 * t10 * t12 * t18 * 24.0F) + COM_X * Jw * Mb * g * q1 *
+            q4 * t2 * t3 * t8 * t18 * 48.0F) + COM_X * Jw * Mb * g * q2 * q3 *
+           t2 * t3 * t8 * t18 * 48.0F) - COM_Z * Jw * Mb * g * q1 * q2 * t2 * t3
+          * t8 * t18 * 72.0F) - COM_X * Jw * Mb * g * q1 * q4 * t2 * t4 * t8 *
+         t18 * 36.0F) - COM_X * Jw * Mb * g * q2 * q3 * t2 * t4 * t8 * t18 *
+    60.0F;
+  f12 = COM_Z * Jw * Mb * g * q1 * q2 * t2 * t4 * t8 * t18 * 84.0F;
+  f13 = COM_X * Jw * Mb * g * q1 * q4 * t3 * t4 * t8 * t18;
+  f14 = COM_X * Jw * Mb * g * q2 * q3 * t3 * t4 * t8 * t18;
+  f15 = COM_Z * Jw * Mb * g * q1 * q2 * t3 * t4 * t8;
+  f16 = COM_X * Jw * Mb * g * q1 * q4 * t2 * t7 * t8;
+  f17 = COM_X * Jw * Mb * g * q2 * q3 * t2 * t7;
+  f18 = COM_Z * Jw * Mb * g * q1 * q2 * t2 * t7;
+  f19 = COM_Z * Jw * Mb * g * q3 * q4 * t2;
+  f20 = COM_X * Jw * Mb * g * q1 * q4 * t3;
+  f21 = COM_X * Jw * Mb * g * q2 * q3;
+  f22 = COM_Z * Jw * Mb * g * q1 * q2;
+  f23 = COM_Z * Jw * Mb * g * q3;
+  f24 = COM_X * Jw * Mb * g * q1;
+  f25 = COM_X * Jw * Mb * g;
+  f26 = COM_Z * Jw * Mb * g;
+  f27 = COM_Z * Jw * Mb;
+  f28 = COM_X * Jw * Mb;
+  f29 = COM_X * Jw;
+  f30 = COM_X * Jw;
+  f11 = ((((((((((((((((((((((((((((((((((((((((((((((((f11 + f12) + f13 * 12.0F)
+    - f14 * 12.0F) + f15 * t18 * 12.0F) - f16 * t18 * 12.0F) + f17 * t8 * t18 *
+    12.0F) + f18 * t8 * t18 * 12.0F) + f19 * t3 * t8 * t18 * 72.0F) - f20 * t7 *
+    t8 * t18 * 60.0F) - f21 * t3 * t7 * t8 * t18 * 36.0F) + f22 * t3 * t7 * t8 *
+    t18 * 84.0F) - f23 * q4 * t2 * t4 * t8 * t18 * 84.0F) + f24 * q4 * t2 * t8 *
+    t9 * t18 * 48.0F) + f25 * q2 * q3 * t2 * t8 * t9 * t18 * 48.0F) - f26 * q1 *
+    q2 * t2 * t8 * t9 * t18 * 72.0F) - f27 * g * q3 * q4 * t3 * t4 * t8 * t18 *
+    12.0F) + f28 * g * q1 * q4 * t2 * t8 * t10 * t18 * 24.0F) - f29 * Mb * g *
+    q1 * q4 * t3 * t8 * t9 * t18 * 24.0F) - f30 * Mb * g * q2 * q3 * t2 * t8 *
+    t10 * t18 * 24.0F) + COM_X * Jw * Mb * g * q2 * q3 * t3 * t8 * t9 * t18 *
+    24.0F) - COM_Z * Jw * Mb * g * q1 * q2 * t2 * t8 * t10 * t18 * 24.0F) -
+    COM_Z * Jw * Mb * g * q1 * q2 * t3 * t8 * t9 * t18 * 24.0F) - COM_X * Jw *
+    Mb * g * q1 * q4 * t2 * t8 * t11 * t18 * 36.0F) + COM_X * Jw * Mb * g * q1 *
+    q4 * t3 * t8 * t10 * t18 * 48.0F) + COM_X * Jw * Mb * g * q1 * q4 * t4 * t8 *
+    t9 * t18 * 36.0F) + COM_X * Jw * Mb * g * q2 * q3 * t2 * t8 * t11 * t18 *
+    36.0F) + COM_X * Jw * Mb * g * q2 * q3 * t3 * t8 * t10 * t18 * 48.0F) -
+    COM_X * Jw * Mb * g * q2 * q3 * t4 * t8 * t9 * t18 * 36.0F) + COM_Z * Jw *
+    Mb * g * q1 * q2 * t2 * t8 * t11 * t18 * 36.0F) - COM_Z * Jw * Mb * g * q1 *
+    q2 * t3 * t8 * t10 * t18 * 72.0F) + COM_Z * Jw * Mb * g * q1 * q2 * t4 * t8 *
+    t9 * t18 * 36.0F) - COM_Z * Jw * Mb * g * q3 * q4 * t2 * t7 * t8 * t18 *
+    12.0F) - COM_X * Jw * Mb * g * q1 * q4 * t2 * t8 * t12 * t18 * 36.0F) -
+                       COM_X * Jw * Mb * g * q1 * q4 * t3 * t8 * t11 * t18 *
+                       60.0F) - COM_X * Jw * Mb * g * q1 * q4 * t4 * t8 * t10 *
+                      t18 * 36.0F) - COM_X * Jw * Mb * g * q2 * q3 * t2 * t8 *
+                     t12 * t18 * 60.0F) - COM_X * Jw * Mb * g * q2 * q3 * t3 *
+                    t8 * t11 * t18 * 36.0F) - COM_X * Jw * Mb * g * q2 * q3 * t4
+                   * t8 * t10 * t18 * 60.0F) + COM_Z * Jw * Mb * g * q1 * q2 *
+                  t2 * t8 * t12 * t18 * 84.0F) + COM_Z * Jw * Mb * g * q1 * q2 *
+                 t3 * t8 * t11 * t18 * 84.0F) + COM_Z * Jw * Mb * g * q1 * q2 *
+                t4 * t8 * t10 * t18 * 84.0F) - COM_Z * Jw * Mb * g * q3 * q4 *
+               t3 * t7 * t8 * t18 * 84.0F) + COM_X * Jw * Mb * g * q1 * q4 * t3 *
+              t8 * t12 * t18 * 36.0F) - COM_X * Jw * Mb * g * q2 * q3 * t3 * t8 *
+             t12 * t18 * 36.0F) + COM_Z * Jw * Mb * g * q1 * q2 * t3 * t8 * t12 *
+            t18 * 36.0F) + COM_Z * Jw * Mb * g * q3 * q4 * t2 * t8 * t9 * t18 *
+           72.0F) - COM_X * Jw * Mb * g * q1 * q4 * t7 * t8 * t9 * t18 * 60.0F)
+         - COM_X * Jw * Mb * g * q2 * q3 * t7 * t8 * t9 * t18 * 36.0F) + COM_Z *
+    Jw * Mb * g * q1 * q2 * t7 * t8 * t9 * t18 * 84.0F;
+  f12 = COM_Z * Jw * Mb * g * q3 * q4 * t2 * t8 * t10 * t18 * 24.0F;
+  f13 = COM_Z * Jw * Mb * g * q3 * q4 * t3 * t8 * t9 * t18 * 24.0F;
+  f14 = COM_X * Jw * Mb * g * q1 * q4 * t7 * t8 * t10 * t18;
+  f15 = COM_X * Jw * Mb * g * q2 * q3 * t7 * t8 * t10 * t18;
+  f16 = COM_Z * Jw * Mb * g * q1 * q2 * t7 * t8 * t10;
+  f17 = COM_Z * Jw * Mb * g * q3 * q4 * t2 * t8 * t11;
+  f18 = COM_Z * Jw * Mb * g * q3 * q4 * t3 * t8;
+  f19 = COM_Z * Jw * Mb * g * q3 * q4 * t4 * t8;
+  f20 = COM_Z * Jw * Mb * g * q3 * q4 * t2;
+  f21 = COM_Z * Jw * Mb * g * q3 * q4 * t3;
+  f22 = COM_Z * Jw * Mb * g * q3 * q4;
+  f23 = COM_X * Jw * Mb * g * q1 * q4;
+  f24 = COM_X * Jw * Mb * g * q2;
+  f25 = COM_Z * Jw * Mb * g * q1;
+  f26 = COM_Z * Jw * Mb * g;
+  f27 = COM_X * Jw * Mb * g;
+  f28 = COM_X * Jw * Mb;
+  f29 = COM_Z * Jw * Mb;
+  f30 = COM_Z * Jw;
+  f31 = COM_X * Jw;
+  f11 = (((((((((((((((((((((((((((((((((((((((((((((((((f11 + f12) + f13) - f14
+    * 36.0F) + f15 * 36.0F) + f16 * t18 * 36.0F) - f17 * t18 * 36.0F) + f18 *
+    t10 * t18 * 72.0F) - f19 * t9 * t18 * 36.0F) - f20 * t8 * t12 * t18 * 84.0F)
+    - f21 * t8 * t11 * t18 * 84.0F) - f22 * t4 * t8 * t10 * t18 * 84.0F) + f23 *
+    t8 * t9 * t10 * t18 * 48.0F) + f24 * q3 * t8 * t9 * t10 * t18 * 48.0F) - f25
+    * q2 * t8 * t9 * t10 * t18 * 72.0F) - f26 * q3 * q4 * t3 * t8 * t12 * t18 *
+    36.0F) - f27 * q1 * q4 * t8 * t9 * t11 * t18 * 60.0F) - f28 * g * q2 * q3 *
+    t8 * t9 * t11 * t18 * 36.0F) + f29 * g * q1 * q2 * t8 * t9 * t11 * t18 *
+    84.0F) - f30 * Mb * g * q3 * q4 * t7 * t8 * t9 * t18 * 84.0F) + f31 * Mb * g
+    * q1 * q4 * t8 * t9 * t12 * t18 * 12.0F) - COM_X * Jw * Mb * g * q1 * q4 *
+    t8 * t10 * t11 * t18 * 12.0F) - COM_X * Jw * Mb * g * q2 * q3 * t8 * t9 *
+    t12 * t18 * 12.0F) + COM_X * Jw * Mb * g * q2 * q3 * t8 * t10 * t11 * t18 *
+    12.0F) + COM_Z * Jw * Mb * g * q1 * q2 * t8 * t9 * t12 * t18 * 12.0F) +
+    COM_Z * Jw * Mb * g * q1 * q2 * t8 * t10 * t11 * t18 * 12.0F) - COM_Z * Jw *
+    Mb * g * q3 * q4 * t7 * t8 * t10 * t18 * 36.0F) - COM_X * Jw * Mb * g * q1 *
+    q4 * t8 * t10 * t12 * t18 * 36.0F) - COM_X * Jw * Mb * g * q2 * q3 * t8 *
+    t10 * t12 * t18 * 60.0F) + COM_Z * Jw * Mb * g * q1 * q2 * t8 * t10 * t12 *
+    t18 * 84.0F) + COM_Z * Jw * Mb * g * q3 * q4 * t8 * t9 * t10 * t18 * 72.0F)
+    - COM_Z * Jw * Mb * g * q3 * q4 * t8 * t9 * t11 * t18 * 84.0F) - COM_Z * Jw *
+    Mb * g * q3 * q4 * t8 * t9 * t12 * t18 * 12.0F) - COM_Z * Jw * Mb * g * q3 *
+    q4 * t8 * t10 * t11 * t18 * 12.0F) - COM_Z * Jw * Mb * g * q3 * q4 * t8 *
+                        t10 * t12 * t18 * 84.0F) - COM_X * Jw * Mb * q2 * q3 *
+                       rk * t2 * t4 * t17 * t18 * 24.0F) + COM_Z * Jw * Mb * q1 *
+                      q2 * rk * t2 * t4 * t17 * t18 * 12.0F) + COM_X * Jw * Mb *
+                     q1 * q4 * rk * t3 * t4 * t17 * t18 * 24.0F) - COM_Z * Jw *
+                    Mb * q1 * q2 * rk * t3 * t4 * t17 * t18 * 12.0F) + COM_X *
+                   Jw * Mb * q2 * q3 * rk * t2 * t7 * t17 * t18 * 24.0F) - COM_Z
+                  * Jw * Mb * q1 * q2 * rk * t2 * t7 * t17 * t18 * 12.0F) -
+                 COM_X * Jw * Mb * q1 * q4 * rk * t3 * t7 * t17 * t18 * 24.0F) +
+                COM_Z * Jw * Mb * q1 * q2 * rk * t3 * t7 * t17 * t18 * 12.0F) -
+               COM_Z * Jw * Mb * q3 * q4 * rk * t2 * t4 * t17 * t18 * 12.0F) +
+              COM_Z * Jw * Mb * q3 * q4 * rk * t3 * t4 * t17 * t18 * 12.0F) +
+             COM_X * Jw * Mb * q1 * q4 * rk * t4 * t9 * t17 * t18 * 72.0F) +
+            COM_X * Jw * Mb * q2 * q3 * rk * t2 * t11 * t17 * t18 * 72.0F) -
+           COM_Z * Jw * Mb * q1 * q2 * rk * t2 * t11 * t17 * t18 * 36.0F) -
+          COM_Z * Jw * Mb * q1 * q2 * rk * t4 * t9 * t17 * t18 * 36.0F) + COM_Z *
+         Jw * Mb * q3 * q4 * rk * t2 * t7 * t17 * t18 * 12.0F) - COM_X * Jw * Mb
+    * q1 * q4 * rk * t3 * t11 * t17 * t18 * 24.0F;
+  f12 = COM_X * Jw * Mb * q2 * q3 * rk * t2 * t12 * t17 * t18 * 24.0F;
+  f13 = COM_X * Jw * Mb * q2 * q3 * rk * t4 * t10 * t17 * t18;
+  f14 = COM_Z * Jw * Mb * q1 * q2 * rk * t2 * t12 * t17 * t18;
+  f15 = COM_Z * Jw * Mb * q1 * q2 * rk * t3 * t11 * t17;
+  f16 = COM_Z * Jw * Mb * q1 * q2 * rk * t4 * t10 * t17;
+  f17 = COM_Z * Jw * Mb * q3 * q4 * rk * t3 * t7;
+  f18 = COM_X * Jw * Mb * q1 * q4 * rk * t3 * t12;
+  f19 = COM_Z * Jw * Mb * q1 * q2 * rk * t3;
+  f20 = COM_X * Jw * Mb * q1 * q4 * rk * t7;
+  f21 = COM_Z * Jw * Mb * q1 * q2 * rk;
+  f22 = COM_X * Jw * Mb * q2 * q3 * rk;
+  f23 = COM_Z * Jw * Mb * q1 * q2;
+  f24 = COM_Z * Jw * Mb * q3 * q4;
+  f25 = COM_Z * Jw * Mb * q3;
+  f26 = COM_Z * Jw * Mb * q3;
+  f27 = COM_Z * Jw * Mb;
+  f28 = COM_Z * Jw * Mb;
+  f29 = COM_Z * Jw;
+  f30 = COM_X * Jw;
+  f11 = ((((((((((((((((((((((((((((((((((((((((((((((((f11 - f12) - f13 * 24.0F)
+    + f14 * 12.0F) + f15 * t18 * 12.0F) + f16 * t18 * 12.0F) - f17 * t17 * t18 *
+    12.0F) + f18 * t17 * t18 * 72.0F) - f19 * t12 * t17 * t18 * 36.0F) - f20 *
+    t9 * t17 * t18 * 24.0F) + f21 * t7 * t9 * t17 * t18 * 12.0F) + f22 * t7 *
+    t10 * t17 * t18 * 72.0F) - f23 * rk * t7 * t10 * t17 * t18 * 36.0F) + f24 *
+    rk * t2 * t11 * t17 * t18 * 36.0F) + f25 * q4 * rk * t4 * t9 * t17 * t18 *
+    36.0F) - f26 * q4 * rk * t2 * t12 * t17 * t18 * 12.0F) - f27 * q3 * q4 * rk *
+    t3 * t11 * t17 * t18 * 12.0F) - f28 * q3 * q4 * rk * t4 * t10 * t17 * t18 *
+    12.0F) + f29 * Mb * q3 * q4 * rk * t3 * t12 * t17 * t18 * 36.0F) - f30 * Mb *
+    q1 * q4 * rk * t9 * t11 * t17 * t18 * 24.0F) + COM_Z * Jw * Mb * q1 * q2 *
+    rk * t9 * t11 * t17 * t18 * 12.0F) - COM_Z * Jw * Mb * q3 * q4 * rk * t7 *
+    t9 * t17 * t18 * 12.0F) + COM_X * Jw * Mb * q1 * q4 * rk * t9 * t12 * t17 *
+    t18 * 24.0F) + COM_X * Jw * Mb * q2 * q3 * rk * t10 * t11 * t17 * t18 *
+    24.0F) - COM_Z * Jw * Mb * q1 * q2 * rk * t9 * t12 * t17 * t18 * 12.0F) -
+    COM_Z * Jw * Mb * q1 * q2 * rk * t10 * t11 * t17 * t18 * 12.0F) + COM_Z * Jw
+    * Mb * q3 * q4 * rk * t7 * t10 * t17 * t18 * 36.0F) - COM_X * Jw * Mb * q2 *
+    q3 * rk * t10 * t12 * t17 * t18 * 24.0F) + COM_Z * Jw * Mb * q1 * q2 * rk *
+    t10 * t12 * t17 * t18 * 12.0F) - COM_Z * Jw * Mb * q3 * q4 * rk * t9 * t11 *
+    t17 * t18 * 12.0F) + COM_Z * Jw * Mb * q3 * q4 * rk * t9 * t12 * t17 * t18 *
+    12.0F) + COM_Z * Jw * Mb * q3 * q4 * rk * t10 * t11 * t17 * t18 * 12.0F) -
+    COM_Z * Jw * Mb * q3 * q4 * rk * t10 * t12 * t17 * t18 * 12.0F) - COM_X *
+                        COM_Y * g * q1 * q3 * rk * t2 * t3 * t19 * t20 * 32.0F)
+                       + COM_X * COM_Y * g * q2 * q4 * rk * t2 * t3 * t19 * t20 *
+                       96.0F) - COM_X * COM_Z * g * q1 * q4 * rk * t2 * t3 * t19
+                      * t20 * 32.0F) + COM_X * COM_Z * g * q2 * q3 * rk * t2 *
+                     t3 * t19 * t20 * 32.0F) + COM_X * COM_Y * g * q1 * q3 * rk *
+                    t2 * t9 * t19 * t20 * 32.0F) + COM_X * COM_Y * g * q1 * q3 *
+                   rk * t2 * t10 * t19 * t20 * 32.0F) + COM_X * COM_Y * g * q1 *
+                  q3 * rk * t3 * t9 * t19 * t20 * 32.0F) + COM_X * COM_Y * g *
+                 q1 * q3 * rk * t3 * t10 * t19 * t20 * 32.0F) + COM_X * COM_Y *
+                g * q2 * q4 * rk * t2 * t9 * t19 * t20 * 32.0F) - COM_X * COM_Z *
+               g * q1 * q4 * rk * t2 * t9 * t19 * t20 * 32.0F) + COM_X * COM_Z *
+              g * q2 * q3 * rk * t2 * t9 * t19 * t20 * 32.0F) + COM_X * COM_Y *
+             g * q2 * q4 * rk * t2 * t10 * t19 * t20 * 32.0F) + COM_X * COM_Y *
+            g * q2 * q4 * rk * t3 * t9 * t19 * t20 * 32.0F) + COM_X * COM_Z * g *
+           q1 * q4 * rk * t2 * t10 * t19 * t20 * 32.0F) - COM_X * COM_Z * g * q1
+          * q4 * rk * t3 * t9 * t19 * t20 * 96.0F) + COM_X * COM_Z * g * q2 * q3
+         * rk * t2 * t10 * t19 * t20 * 96.0F) - COM_X * COM_Z * g * q2 * q3 * rk
+    * t3 * t9 * t19 * t20 * 32.0F;
+  f12 = COM_X * COM_Y * g * q2 * q4 * rk * t3 * t10 * t19 * t20 * 32.0F;
+  f13 = COM_X * COM_Z * g * q1 * q4 * rk * t3 * t10 * t19 * t20 * 32.0F;
+  f14 = COM_X * COM_Z * g * q2 * q3 * rk * t3 * t10 * t19 * t20;
+  f15 = COM_X * COM_Y * g * q1 * q3 * rk * t9 * t10 * t19 * t20;
+  f16 = COM_X * COM_Y * g * q2 * q4 * rk * t9 * t10 * t19;
+  f17 = COM_X * COM_Z * g * q1 * q4 * rk * t9 * t10 * t19;
+  f18 = COM_X * COM_Z * g * q2 * q3 * rk * t9 * t10;
+  f19 = COM_Y * Jw * Mb * dx * dy * q2 * q3 * rk * t2;
+  f20 = COM_Z * Jw * Mb * dx * dy * q1 * q3 * rk;
+  f21 = COM_Y * Jw * Mb * dx * dy * q1 * q4 * rk;
+  f22 = COM_Z * Jw * Mb * dx * dy * q1 * q3;
+  f23 = COM_Z * Jw * Mb * dx * dy * q2 * q4;
+  f24 = COM_Y * Jw * Mb * dx * dy * q2;
+  f25 = COM_Z * Jw * Mb * dx * dy * q1;
+  f26 = COM_Z * Jw * Mb * dx * dy;
+  f27 = COM_Y * Jw * Mb * dx * dy;
+  f28 = COM_Z * Jw * Mb * dx;
+  f29 = COM_Z * Jw * Mb * dx;
+  f30 = COM_Z * Jw * Mb;
+  f31 = COM_Y * Jw * Mb;
+  t24 = COM_Y * Jw;
+  f32 = COM_Z * Jw;
+  f11 = (((((((((((((((((((((((((((((((((((((((((((((((((f11 + f12) - f13) + f14
+    * 32.0F) + f15 * 96.0F) - f16 * t20 * 32.0F) - f17 * t20 * 32.0F) + f18 *
+    t19 * t20 * 32.0F) + f19 * t4 * t18 * 24.0F) + f20 * t2 * t4 * t18 * 12.0F)
+    + f21 * t3 * t4 * t18 * 24.0F) - f22 * rk * t3 * t4 * t18 * 12.0F) + f23 *
+    rk * t2 * t4 * t18 * 12.0F) - f24 * q3 * rk * t2 * t7 * t18 * 24.0F) - f25 *
+    q3 * rk * t2 * t7 * t18 * 12.0F) - f26 * q2 * q4 * rk * t3 * t4 * t18 *
+    12.0F) - f27 * q1 * q4 * rk * t3 * t7 * t18 * 24.0F) + f28 * dy * q1 * q3 *
+    rk * t3 * t7 * t18 * 12.0F) - f29 * dy * q2 * q4 * rk * t2 * t7 * t18 *
+    12.0F) + f30 * dx * dy * q2 * q4 * rk * t3 * t7 * t18 * 12.0F) + f31 * dx *
+    dy * q1 * q4 * rk * t4 * t9 * t18 * 72.0F) - t24 * Mb * dx * dy * q2 * q3 *
+    rk * t2 * t11 * t18 * 72.0F) - f32 * Mb * dx * dy * q1 * q3 * rk * t2 * t11 *
+    t18 * 36.0F) - COM_Z * Jw * Mb * dx * dy * q1 * q3 * rk * t4 * t9 * t18 *
+    36.0F) - COM_Y * Jw * Mb * dx * dy * q1 * q4 * rk * t3 * t11 * t18 * 24.0F)
+    + COM_Y * Jw * Mb * dx * dy * q2 * q3 * rk * t2 * t12 * t18 * 24.0F) + COM_Y
+    * Jw * Mb * dx * dy * q2 * q3 * rk * t4 * t10 * t18 * 24.0F) + COM_Z * Jw *
+    Mb * dx * dy * q1 * q3 * rk * t2 * t12 * t18 * 12.0F) + COM_Z * Jw * Mb * dx
+    * dy * q1 * q3 * rk * t3 * t11 * t18 * 12.0F) + COM_Z * Jw * Mb * dx * dy *
+    q1 * q3 * rk * t4 * t10 * t18 * 12.0F) + COM_Y * Jw * Mb * dx * dy * q1 * q4
+    * rk * t3 * t12 * t18 * 72.0F) - COM_Z * Jw * Mb * dx * dy * q1 * q3 * rk *
+    t3 * t12 * t18 * 36.0F) - COM_Z * Jw * Mb * dx * dy * q2 * q4 * rk * t2 *
+    t11 * t18 * 36.0F) - COM_Z * Jw * Mb * dx * dy * q2 * q4 * rk * t4 * t9 *
+    t18 * 36.0F) - COM_Y * Jw * Mb * dx * dy * q1 * q4 * rk * t7 * t9 * t18 *
+                        24.0F) + COM_Z * Jw * Mb * dx * dy * q1 * q3 * rk * t7 *
+                       t9 * t18 * 12.0F) + COM_Z * Jw * Mb * dx * dy * q2 * q4 *
+                      rk * t2 * t12 * t18 * 12.0F) + COM_Z * Jw * Mb * dx * dy *
+                     q2 * q4 * rk * t3 * t11 * t18 * 12.0F) + COM_Z * Jw * Mb *
+                    dx * dy * q2 * q4 * rk * t4 * t10 * t18 * 12.0F) - COM_Y *
+                   Jw * Mb * dx * dy * q2 * q3 * rk * t7 * t10 * t18 * 72.0F) -
+                  COM_Z * Jw * Mb * dx * dy * q1 * q3 * rk * t7 * t10 * t18 *
+                  36.0F) - COM_Z * Jw * Mb * dx * dy * q2 * q4 * rk * t3 * t12 *
+                 t18 * 36.0F) + COM_Z * Jw * Mb * dx * dy * q2 * q4 * rk * t7 *
+                t9 * t18 * 12.0F) - COM_Z * Jw * Mb * dx * dy * q2 * q4 * rk *
+               t7 * t10 * t18 * 36.0F) - COM_Y * Jw * Mb * dx * dy * q1 * q4 *
+              rk * t9 * t11 * t18 * 24.0F) + COM_Z * Jw * Mb * dx * dy * q1 * q3
+             * rk * t9 * t11 * t18 * 12.0F) + COM_Y * Jw * Mb * dx * dy * q1 *
+            q4 * rk * t9 * t12 * t18 * 24.0F) - COM_Y * Jw * Mb * dx * dy * q2 *
+           q3 * rk * t10 * t11 * t18 * 24.0F) - COM_Z * Jw * Mb * dx * dy * q1 *
+          q3 * rk * t9 * t12 * t18 * 12.0F) - COM_Z * Jw * Mb * dx * dy * q1 *
+         q3 * rk * t10 * t11 * t18 * 12.0F) + COM_Y * Jw * Mb * dx * dy * q2 *
+    q3 * rk * t10 * t12 * t18 * 24.0F;
+  f12 = COM_Z * Jw * Mb * dx * dy * q1 * q3 * rk * t10 * t12 * t18 * 12.0F;
+  f13 = COM_Z * Jw * Mb * dx * dy * q2 * q4 * rk * t9 * t11 * t18;
+  f14 = COM_Z * Jw * Mb * dx * dy * q2 * q4 * rk * t9 * t12 * t18;
+  f15 = COM_Z * Jw * Mb * dx * dy * q2 * q4 * rk * t10 * t11;
+  f16 = COM_Z * Jw * Mb * dx * dy * q2 * q4 * rk * t10 * t12;
+  f17 = COM_X * Jw * Mb * g * q1 * q4 * t2 * t3 * t8;
+  f18 = COM_X * Jw * Mb * g * q2 * q3 * t2 * t3 * t8;
+  f19 = COM_Z * Jw * Mb * g * q1 * q2 * t2 * t3;
+  f20 = COM_X * Jw * Mb * g * q1 * q4 * t2 * t3;
+  f21 = COM_X * Jw * Mb * g * q2 * q3 * t2;
+  f22 = COM_Z * Jw * Mb * g * q1 * q2 * t2;
+  f23 = COM_Z * Jw * Mb * g * q3 * q4;
+  f24 = COM_Z * Jw * Mb * g * q3 * q4;
+  f25 = COM_X * Jw * Mb * g * q1;
+  f26 = COM_X * Jw * Mb * g * q2;
+  f27 = COM_Z * Jw * Mb * g;
+  f28 = COM_X * Jw * Mb * g;
+  f29 = COM_X * Jw * Mb;
+  f30 = COM_Z * Jw * Mb;
+  f31 = COM_Z * Jw;
+  t24 = COM_Z * Jw;
+  acceleration[1] = rk * t476 * (((((((((((((((((((((((((((((((((((((((((((((f11
+    + f12) + f13 * 12.0F) - f14 * 12.0F) - f15 * t18 * 12.0F) + f16 * t18 *
+    12.0F) - f17 * t9 * t18 * 72.0F) - f18 * t9 * t18 * 120.0F) + f19 * t8 * t9 *
+    t18 * 168.0F) - f20 * t8 * t10 * t18 * 120.0F) - f21 * t3 * t8 * t10 * t18 *
+    72.0F) + f22 * t3 * t8 * t10 * t18 * 168.0F) - f23 * t2 * t3 * t8 * t9 * t18
+    * 168.0F) - f24 * t2 * t3 * t8 * t10 * t18 * 168.0F) - f25 * q4 * t2 * t8 *
+    t9 * t10 * t18 * 120.0F) - f26 * q3 * t2 * t8 * t9 * t10 * t18 * 72.0F) +
+    f27 * q1 * q2 * t2 * t8 * t9 * t10 * t18 * 168.0F) - f28 * q1 * q4 * t3 * t8
+    * t9 * t10 * t18 * 72.0F) - f29 * g * q2 * q3 * t3 * t8 * t9 * t10 * t18 *
+    120.0F) + f30 * g * q1 * q2 * t3 * t8 * t9 * t10 * t18 * 168.0F) - f31 * Mb *
+    g * q3 * q4 * t2 * t8 * t9 * t10 * t18 * 168.0F) - t24 * Mb * g * q3 * q4 *
+    t3 * t8 * t9 * t10 * t18 * 168.0F) - COM_X * Jw * Mb * q2 * q3 * rk * t2 *
+    t3 * t9 * t17 * t18 * 48.0F) + COM_Z * Jw * Mb * q1 * q2 * rk * t2 * t3 * t9
+    * t17 * t18 * 24.0F) - COM_X * Jw * Mb * q1 * q4 * rk * t2 * t3 * t10 * t17 *
+    t18 * 48.0F) + COM_Z * Jw * Mb * q1 * q2 * rk * t2 * t3 * t10 * t17 * t18 *
+    24.0F) - COM_Z * Jw * Mb * q3 * q4 * rk * t2 * t3 * t9 * t17 * t18 * 24.0F)
+    - COM_Z * Jw * Mb * q3 * q4 * rk * t2 * t3 * t10 * t17 * t18 * 24.0F) -
+    COM_X * Jw * Mb * q1 * q4 * rk * t2 * t9 * t10 * t17 * t18 * 48.0F) + COM_Z *
+    Jw * Mb * q1 * q2 * rk * t2 * t9 * t10 * t17 * t18 * 24.0F) - COM_X * Jw *
+    Mb * q2 * q3 * rk * t3 * t9 * t10 * t17 * t18 * 48.0F) + COM_Z * Jw * Mb *
+    q1 * q2 * rk * t3 * t9 * t10 * t17 * t18 * 24.0F) - COM_Z * Jw * Mb * q3 *
+    q4 * rk * t2 * t9 * t10 * t17 * t18 * 24.0F) - COM_Z * Jw * Mb * q3 * q4 *
+    rk * t3 * t9 * t10 * t17 * t18 * 24.0F) + COM_Y * Jw * Mb * dx * dy * q2 *
+    q3 * rk * t2 * t3 * t9 * t18 * 48.0F) + COM_Z * Jw * Mb * dx * dy * q1 * q3 *
+    rk * t2 * t3 * t9 * t18 * 24.0F) - COM_Y * Jw * Mb * dx * dy * q1 * q4 * rk *
+    t2 * t3 * t10 * t18 * 48.0F) + COM_Z * Jw * Mb * dx * dy * q1 * q3 * rk * t2
+    * t3 * t10 * t18 * 24.0F) + COM_Z * Jw * Mb * dx * dy * q2 * q4 * rk * t2 *
+    t3 * t9 * t18 * 24.0F) + COM_Z * Jw * Mb * dx * dy * q2 * q4 * rk * t2 * t3 *
+    t10 * t18 * 24.0F) - COM_Y * Jw * Mb * dx * dy * q1 * q4 * rk * t2 * t9 *
+    t10 * t18 * 48.0F) + COM_Z * Jw * Mb * dx * dy * q1 * q3 * rk * t2 * t9 *
+    t10 * t18 * 24.0F) + COM_Y * Jw * Mb * dx * dy * q2 * q3 * rk * t3 * t9 *
+    t10 * t18 * 48.0F) + COM_Z * Jw * Mb * dx * dy * q1 * q3 * rk * t3 * t9 *
+    t10 * t18 * 24.0F) + COM_Z * Jw * Mb * dx * dy * q2 * q4 * rk * t2 * t9 *
+    t10 * t18 * 24.0F) + COM_Z * Jw * Mb * dx * dy * q2 * q4 * rk * t3 * t9 *
+    t10 * t18 * 24.0F) * 2.0F;
+}
+
+//
+// File trailer for SteadyStateAcceleration.cpp
+//
+// [EOF]
+//
diff --git a/KugleFirmware/Libraries/Modules/Estimators/VelocityEKF/MATLABCoder/SteadyStateAcceleration.h b/KugleFirmware/Libraries/Modules/Estimators/VelocityEKF/MATLABCoder/SteadyStateAcceleration.h
new file mode 100644
index 0000000..dd952cc
--- /dev/null
+++ b/KugleFirmware/Libraries/Modules/Estimators/VelocityEKF/MATLABCoder/SteadyStateAcceleration.h
@@ -0,0 +1,30 @@
+//
+// Academic License - for use in teaching, academic research, and meeting
+// course requirements at degree granting institutions only.  Not for
+// government, commercial, or other organizational use.
+// File: SteadyStateAcceleration.h
+//
+// MATLAB Coder version            : 4.0
+// C/C++ source code generated on  : 19-Nov-2018 11:57:59
+//
+#ifndef STEADYSTATEACCELERATION_H
+#define STEADYSTATEACCELERATION_H
+
+// Include Files
+#include <stddef.h>
+#include <stdlib.h>
+#include "rtwtypes.h"
+#include "VelocityEstimator_types.h"
+
+// Function Declarations
+extern void SteadyStateAcceleration(float COM_X, float COM_Y, float COM_Z, float
+  Jk, float Jw, float Mb, float Mk, float dx, float dy, float g, float q1, float
+  q2, float q3, float q4, float rk, float rw, float acceleration[2]);
+
+#endif
+
+//
+// File trailer for SteadyStateAcceleration.h
+//
+// [EOF]
+//
diff --git a/KugleFirmware/Libraries/Modules/Estimators/VelocityEKF/MATLABCoder/SteadyStateAcceleration_dCOM.cpp b/KugleFirmware/Libraries/Modules/Estimators/VelocityEKF/MATLABCoder/SteadyStateAcceleration_dCOM.cpp
new file mode 100644
index 0000000..bb04b40
--- /dev/null
+++ b/KugleFirmware/Libraries/Modules/Estimators/VelocityEKF/MATLABCoder/SteadyStateAcceleration_dCOM.cpp
@@ -0,0 +1,364 @@
+//
+// Academic License - for use in teaching, academic research, and meeting
+// course requirements at degree granting institutions only.  Not for
+// government, commercial, or other organizational use.
+// File: SteadyStateAcceleration_dCOM.cpp
+//
+// MATLAB Coder version            : 4.0
+// C/C++ source code generated on  : 19-Nov-2018 11:57:59
+//
+
+// Include Files
+#include "rt_nonfinite.h"
+#include "VelocityEstimator.h"
+#include "SteadyStateAcceleration_dCOM.h"
+
+// Function Definitions
+
+//
+// function dAcceleration_dCOM = SteadyStateAcceleration_dCOM(COM_X,COM_Y,COM_Z,Jk,Mb,Mk,g,q1,q2,q3,q4,rk)
+// STEADYSTATEACCELERATION_DCOM
+//     DACCELERATION_DCOM = STEADYSTATEACCELERATION_DCOM(COM_X,COM_Y,COM_Z,JK,MB,MK,G,Q1,Q2,Q3,Q4,RK)
+// Arguments    : float COM_X
+//                float COM_Y
+//                float COM_Z
+//                float Jk
+//                float Mb
+//                float Mk
+//                float g
+//                float q1
+//                float q2
+//                float q3
+//                float q4
+//                float rk
+//                float dAcceleration_dCOM[4]
+// Return Type  : void
+//
+__attribute__((optimize("O3"))) void SteadyStateAcceleration_dCOM(float COM_X, float COM_Y, float COM_Z, float
+  Jk, float Mb, float Mk, float g, float q1, float q2, float q3, float q4, float
+  rk, float dAcceleration_dCOM[4])
+{
+  float t2;
+  float t3;
+  float t4;
+  float t5;
+  float t6;
+  float t7;
+  float t8;
+  float t9;
+  float t10;
+  float t28;
+  float t37;
+  float t38;
+  float t39;
+  float t40;
+  float t41;
+  float t42;
+  float t43;
+  float t44;
+  float t45;
+  float t46;
+  float t47;
+  float t48;
+  float t49;
+  float t50;
+  float t51;
+  float t52;
+  float t53;
+  float t54;
+  float t55;
+  float t56;
+  float t57;
+  float t58;
+  float t59;
+  float t60;
+  float t61;
+  float t62;
+  float t63;
+  float t64;
+  float t65;
+  float t66;
+  float t67;
+  float t68;
+  float t69;
+  float t70;
+  float t71;
+  float f10;
+  float x[4];
+  int i4;
+  int i5;
+  int i6;
+  int i7;
+
+  //     This function was generated by the Symbolic Math Toolbox version 8.1.
+  //     16-Nov-2018 12:04:30
+  // 'SteadyStateAcceleration_dCOM:8' t2 = rk.^2;
+  t2 = rk * rk;
+
+  // 'SteadyStateAcceleration_dCOM:9' t3 = q1.^2;
+  t3 = q1 * q1;
+
+  // 'SteadyStateAcceleration_dCOM:10' t4 = q2.^2;
+  t4 = q2 * q2;
+
+  // 'SteadyStateAcceleration_dCOM:11' t5 = q3.^2;
+  t5 = q3 * q3;
+
+  // 'SteadyStateAcceleration_dCOM:12' t6 = q4.^2;
+  t6 = q4 * q4;
+
+  // 'SteadyStateAcceleration_dCOM:13' t7 = t3.^2;
+  t7 = t3 * t3;
+
+  // 'SteadyStateAcceleration_dCOM:14' t8 = t4.^2;
+  t8 = t4 * t4;
+
+  // 'SteadyStateAcceleration_dCOM:15' t9 = t5.^2;
+  t9 = t5 * t5;
+
+  // 'SteadyStateAcceleration_dCOM:16' t10 = t6.^2;
+  t10 = t6 * t6;
+
+  // 'SteadyStateAcceleration_dCOM:17' t11 = Mb.*t2;
+  // 'SteadyStateAcceleration_dCOM:18' t12 = Mk.*t2;
+  // 'SteadyStateAcceleration_dCOM:19' t13 = COM_Z.*Mb.*rk.*t7;
+  // 'SteadyStateAcceleration_dCOM:20' t14 = COM_Z.*Mb.*rk.*t10;
+  // 'SteadyStateAcceleration_dCOM:21' t15 = COM_Z.*Mb.*rk.*t3.*t6.*2.0;
+  // 'SteadyStateAcceleration_dCOM:22' t16 = COM_Y.*Mb.*q1.*q2.*rk.*t4.*2.0;
+  // 'SteadyStateAcceleration_dCOM:23' t17 = COM_Y.*Mb.*q1.*q2.*rk.*t3.*2.0;
+  // 'SteadyStateAcceleration_dCOM:24' t18 = COM_X.*Mb.*q2.*q4.*rk.*t6.*2.0;
+  // 'SteadyStateAcceleration_dCOM:25' t19 = COM_X.*Mb.*q2.*q4.*rk.*t4.*2.0;
+  // 'SteadyStateAcceleration_dCOM:26' t20 = COM_Y.*Mb.*q3.*q4.*rk.*t6.*2.0;
+  // 'SteadyStateAcceleration_dCOM:27' t21 = COM_Y.*Mb.*q3.*q4.*rk.*t5.*2.0;
+  // 'SteadyStateAcceleration_dCOM:28' t22 = COM_X.*Mb.*q2.*q4.*rk.*t3.*2.0;
+  // 'SteadyStateAcceleration_dCOM:29' t23 = COM_Y.*Mb.*q1.*q2.*rk.*t5.*2.0;
+  // 'SteadyStateAcceleration_dCOM:30' t24 = COM_Y.*Mb.*q1.*q2.*rk.*t6.*2.0;
+  // 'SteadyStateAcceleration_dCOM:31' t25 = COM_X.*Mb.*q2.*q4.*rk.*t5.*2.0;
+  // 'SteadyStateAcceleration_dCOM:32' t26 = COM_Y.*Mb.*q3.*q4.*rk.*t3.*2.0;
+  // 'SteadyStateAcceleration_dCOM:33' t27 = COM_Y.*Mb.*q3.*q4.*rk.*t4.*2.0;
+  // 'SteadyStateAcceleration_dCOM:34' t30 = COM_Z.*Mb.*rk.*t8;
+  // 'SteadyStateAcceleration_dCOM:35' t31 = COM_Z.*Mb.*rk.*t9;
+  // 'SteadyStateAcceleration_dCOM:36' t32 = COM_Z.*Mb.*rk.*t4.*t5.*2.0;
+  // 'SteadyStateAcceleration_dCOM:37' t33 = COM_X.*Mb.*q1.*q3.*rk.*t5.*2.0;
+  // 'SteadyStateAcceleration_dCOM:38' t34 = COM_X.*Mb.*q1.*q3.*rk.*t3.*2.0;
+  // 'SteadyStateAcceleration_dCOM:39' t35 = COM_X.*Mb.*q1.*q3.*rk.*t4.*2.0;
+  // 'SteadyStateAcceleration_dCOM:40' t36 = COM_X.*Mb.*q1.*q3.*rk.*t6.*2.0;
+  // 'SteadyStateAcceleration_dCOM:41' t28 = Jk+t11+t12+t13+t14+t15+t16+t17+t18+t19+t20+t21+t22+t23+t24+t25+t26+t27-t30-t31-t32-t33-t34-t35-t36; 
+  t28 = (((((((((((((((((((((((Jk + Mb * t2) + Mk * t2) + COM_Z * Mb * rk * t7)
+    + COM_Z * Mb * rk * t10) + COM_Z * Mb * rk * t3 * t6 * 2.0F) + COM_Y * Mb *
+    q1 * q2 * rk * t4 * 2.0F) + COM_Y * Mb * q1 * q2 * rk * t3 * 2.0F) + COM_X *
+                        Mb * q2 * q4 * rk * t6 * 2.0F) + COM_X * Mb * q2 * q4 *
+                       rk * t4 * 2.0F) + COM_Y * Mb * q3 * q4 * rk * t6 * 2.0F)
+                     + COM_Y * Mb * q3 * q4 * rk * t5 * 2.0F) + COM_X * Mb * q2 *
+                    q4 * rk * t3 * 2.0F) + COM_Y * Mb * q1 * q2 * rk * t5 * 2.0F)
+                  + COM_Y * Mb * q1 * q2 * rk * t6 * 2.0F) + COM_X * Mb * q2 *
+                 q4 * rk * t5 * 2.0F) + COM_Y * Mb * q3 * q4 * rk * t3 * 2.0F) +
+               COM_Y * Mb * q3 * q4 * rk * t4 * 2.0F) - COM_Z * Mb * rk * t8) -
+             COM_Z * Mb * rk * t9) - COM_Z * Mb * rk * t4 * t5 * 2.0F) - COM_X *
+           Mb * q1 * q3 * rk * t5 * 2.0F) - COM_X * Mb * q1 * q3 * rk * t3 *
+          2.0F) - COM_X * Mb * q1 * q3 * rk * t4 * 2.0F) - COM_X * Mb * q1 * q3 *
+    rk * t6 * 2.0F;
+
+  // 'SteadyStateAcceleration_dCOM:42' t29 = 1.0./t28.^2;
+  t28 = 1.0F / (t28 * t28);
+
+  // 'SteadyStateAcceleration_dCOM:43' t37 = Jk.*q1.*q4;
+  t37 = Jk * q1 * q4;
+
+  // 'SteadyStateAcceleration_dCOM:44' t38 = Mb.*q1.*q4.*t2;
+  t38 = Mb * q1 * q4 * t2;
+
+  // 'SteadyStateAcceleration_dCOM:45' t39 = Mk.*q1.*q4.*t2;
+  t39 = Mk * q1 * q4 * t2;
+
+  // 'SteadyStateAcceleration_dCOM:46' t40 = COM_Z.*Mb.*q1.*q4.*rk.*t3.*t6.*2.0; 
+  t40 = COM_Z * Mb * q1 * q4 * rk * t3 * t6 * 2.0F;
+
+  // 'SteadyStateAcceleration_dCOM:47' t41 = COM_Z.*Mb.*q2.*q3.*rk.*t4.*t5.*2.0; 
+  t41 = COM_Z * Mb * q2 * q3 * rk * t4 * t5 * 2.0F;
+
+  // 'SteadyStateAcceleration_dCOM:48' t42 = COM_Z.*Mb.*q1.*q4.*rk.*t10;
+  t42 = COM_Z * Mb * q1 * q4 * rk * t10;
+
+  // 'SteadyStateAcceleration_dCOM:49' t43 = COM_Z.*Mb.*q2.*q3.*rk.*t9;
+  t43 = COM_Z * Mb * q2 * q3 * rk * t9;
+
+  // 'SteadyStateAcceleration_dCOM:50' t44 = COM_Z.*Mb.*q1.*q4.*rk.*t7;
+  t44 = COM_Z * Mb * q1 * q4 * rk * t7;
+
+  // 'SteadyStateAcceleration_dCOM:51' t45 = COM_Z.*Mb.*q2.*q3.*rk.*t8;
+  t45 = COM_Z * Mb * q2 * q3 * rk * t8;
+
+  // 'SteadyStateAcceleration_dCOM:52' t46 = COM_Z.*Mb.*q2.*q3.*rk.*t7;
+  t46 = COM_Z * Mb * q2 * q3 * rk * t7;
+
+  // 'SteadyStateAcceleration_dCOM:53' t47 = COM_Z.*Mb.*q1.*q4.*rk.*t8;
+  t47 = COM_Z * Mb * q1 * q4 * rk * t8;
+
+  // 'SteadyStateAcceleration_dCOM:54' t48 = COM_Z.*Mb.*q1.*q4.*rk.*t9;
+  t48 = COM_Z * Mb * q1 * q4 * rk * t9;
+
+  // 'SteadyStateAcceleration_dCOM:55' t49 = COM_Z.*Mb.*q2.*q3.*rk.*t10;
+  t49 = COM_Z * Mb * q2 * q3 * rk * t10;
+
+  // 'SteadyStateAcceleration_dCOM:56' t50 = COM_Z.*Mb.*q2.*q3.*rk.*t3.*t5.*2.0; 
+  t50 = COM_Z * Mb * q2 * q3 * rk * t3 * t5 * 2.0F;
+
+  // 'SteadyStateAcceleration_dCOM:57' t51 = COM_Z.*Mb.*q2.*q3.*rk.*t3.*t4.*2.0; 
+  t51 = COM_Z * Mb * q2 * q3 * rk * t3 * t4 * 2.0F;
+
+  // 'SteadyStateAcceleration_dCOM:58' t52 = COM_Z.*Mb.*q1.*q4.*rk.*t4.*t6.*2.0; 
+  t52 = COM_Z * Mb * q1 * q4 * rk * t4 * t6 * 2.0F;
+
+  // 'SteadyStateAcceleration_dCOM:59' t53 = COM_Z.*Mb.*q1.*q4.*rk.*t3.*t4.*2.0; 
+  t53 = COM_Z * Mb * q1 * q4 * rk * t3 * t4 * 2.0F;
+
+  // 'SteadyStateAcceleration_dCOM:60' t54 = COM_Z.*Mb.*q1.*q4.*rk.*t5.*t6.*2.0; 
+  t54 = COM_Z * Mb * q1 * q4 * rk * t5 * t6 * 2.0F;
+
+  // 'SteadyStateAcceleration_dCOM:61' t55 = COM_Z.*Mb.*q1.*q4.*rk.*t3.*t5.*2.0; 
+  t55 = COM_Z * Mb * q1 * q4 * rk * t3 * t5 * 2.0F;
+
+  // 'SteadyStateAcceleration_dCOM:62' t56 = COM_Z.*Mb.*q2.*q3.*rk.*t5.*t6.*2.0; 
+  t56 = COM_Z * Mb * q2 * q3 * rk * t5 * t6 * 2.0F;
+
+  // 'SteadyStateAcceleration_dCOM:63' t57 = COM_Z.*Mb.*q2.*q3.*rk.*t4.*t6.*2.0; 
+  t57 = COM_Z * Mb * q2 * q3 * rk * t4 * t6 * 2.0F;
+
+  // 'SteadyStateAcceleration_dCOM:64' t58 = COM_Z.*Mb.*q1.*q4.*rk.*t4.*t5.*2.0; 
+  t58 = COM_Z * Mb * q1 * q4 * rk * t4 * t5 * 2.0F;
+
+  // 'SteadyStateAcceleration_dCOM:65' t59 = COM_Z.*Mb.*q2.*q3.*rk.*t3.*t6.*2.0; 
+  t59 = COM_Z * Mb * q2 * q3 * rk * t3 * t6 * 2.0F;
+
+  // 'SteadyStateAcceleration_dCOM:66' t60 = Jk.*t3;
+  t60 = Jk * t3;
+
+  // 'SteadyStateAcceleration_dCOM:67' t61 = Jk.*t4;
+  t61 = Jk * t4;
+
+  // 'SteadyStateAcceleration_dCOM:68' t62 = Mb.*t2.*t3;
+  t62 = Mb * t2 * t3;
+
+  // 'SteadyStateAcceleration_dCOM:69' t63 = Mb.*t2.*t4;
+  t63 = Mb * t2 * t4;
+
+  // 'SteadyStateAcceleration_dCOM:70' t64 = Mk.*t2.*t3;
+  t64 = Mk * t2 * t3;
+
+  // 'SteadyStateAcceleration_dCOM:71' t65 = Mk.*t2.*t4;
+  t65 = Mk * t2 * t4;
+
+  // 'SteadyStateAcceleration_dCOM:72' t66 = COM_Z.*Mb.*rk.*t3.*t7;
+  t66 = COM_Z * Mb * rk * t3 * t7;
+
+  // 'SteadyStateAcceleration_dCOM:73' t67 = COM_Z.*Mb.*rk.*t5.*t9;
+  t67 = COM_Z * Mb * rk * t5 * t9;
+
+  // 'SteadyStateAcceleration_dCOM:74' t68 = COM_Z.*Mb.*rk.*t4.*t9;
+  t68 = COM_Z * Mb * rk * t4 * t9;
+
+  // 'SteadyStateAcceleration_dCOM:75' t69 = COM_Z.*Mb.*rk.*t6.*t7;
+  t69 = COM_Z * Mb * rk * t6 * t7;
+
+  // 'SteadyStateAcceleration_dCOM:76' t70 = COM_Z.*Mb.*rk.*t3.*t4.*t5.*2.0;
+  t70 = COM_Z * Mb * rk * t3 * t4 * t5 * 2.0F;
+
+  // 'SteadyStateAcceleration_dCOM:77' t71 = COM_Z.*Mb.*rk.*t3.*t5.*t6.*2.0;
+  t71 = COM_Z * Mb * rk * t3 * t5 * t6 * 2.0F;
+
+  // 'SteadyStateAcceleration_dCOM:78' dAcceleration_dCOM = reshape([Mb.*g.*rk.*t29.*(t60+t61+t62+t63+t64+t65+t66+t67+t68+t69+t70+t71-Jk.*t5-Jk.*t6-Mb.*t2.*t5-Mb.*t2.*t6-Mk.*t2.*t5-Mk.*t2.*t6-COM_Z.*Mb.*rk.*t3.*t8+COM_Z.*Mb.*rk.*t4.*t7+COM_Z.*Mb.*rk.*t3.*t9.*3.0-COM_Z.*Mb.*rk.*t4.*t8+COM_Z.*Mb.*rk.*t5.*t7.*3.0-COM_Z.*Mb.*rk.*t3.*t10-COM_Z.*Mb.*rk.*t5.*t8-COM_Z.*Mb.*rk.*t4.*t10.*3.0-COM_Z.*Mb.*rk.*t6.*t8.*3.0-COM_Z.*Mb.*rk.*t5.*t10+COM_Z.*Mb.*rk.*t6.*t9-COM_Z.*Mb.*rk.*t6.*t10+COM_Y.*Mb.*q1.*q2.*rk.*t7.*2.0+COM_Y.*Mb.*q1.*q2.*rk.*t8.*2.0+COM_Y.*Mb.*q1.*q2.*rk.*t9.*2.0+COM_Y.*Mb.*q1.*q2.*rk.*t10.*2.0-COM_Y.*Mb.*q3.*q4.*rk.*t7.*2.0-COM_Y.*Mb.*q3.*q4.*rk.*t8.*2.0-COM_Y.*Mb.*q3.*q4.*rk.*t9.*2.0-COM_Y.*Mb.*q3.*q4.*rk.*t10.*2.0-COM_Z.*Mb.*rk.*t3.*t4.*t6.*2.0-COM_Z.*Mb.*rk.*t4.*t5.*t6.*2.0+COM_Y.*Mb.*q1.*q2.*rk.*t3.*t4.*4.0+COM_Y.*Mb.*q1.*q2.*rk.*t3.*t5.*4.0+COM_Y.*Mb.*q1.*q2.*rk.*t3.*t6.*4.0+COM_Y.*Mb.*q1.*q2.*rk.*t4.*t5.*4.0+COM_Y.*Mb.*q1.*q2.*rk.*t4.*t6.*4.0+COM_Y.*Mb.*q1.*q2.*rk.*t5.*t6.*4.0-COM_Y.*Mb.*q3.*q4.*rk.*t3.*t4.*4.0-COM_Y.*Mb.*q3.*q4.*rk.*t3.*t5.*4.0-COM_Y.*Mb.*q3.*q4.*rk.*t3.*t6.*4.0-COM_Y.*Mb.*q3.*q4.*rk.*t4.*t5.*4.0-COM_Y.*Mb.*q3.*q4.*rk.*t4.*t6.*4.0-COM_Y.*Mb.*q3.*q4.*rk.*t5.*t6.*4.0),Mb.*g.*rk.*t29.*(t37+t38+t39+t40-t41+t42-t43+t44-t45-t46+t47+t48-t49-t50-t51+t52+t53+t54+t55-t56-t57+t58-t59+Jk.*q2.*q3+Mb.*q2.*q3.*t2+Mk.*q2.*q3.*t2+COM_Y.*Mb.*q1.*q3.*rk.*t7+COM_Y.*Mb.*q1.*q3.*rk.*t8+COM_Y.*Mb.*q1.*q3.*rk.*t9+COM_Y.*Mb.*q2.*q4.*rk.*t7+COM_Y.*Mb.*q1.*q3.*rk.*t10+COM_Y.*Mb.*q2.*q4.*rk.*t8+COM_Y.*Mb.*q2.*q4.*rk.*t9+COM_Y.*Mb.*q2.*q4.*rk.*t10+COM_Y.*Mb.*q1.*q3.*rk.*t3.*t4.*2.0+COM_Y.*Mb.*q1.*q3.*rk.*t3.*t5.*2.0+COM_Y.*Mb.*q1.*q3.*rk.*t3.*t6.*2.0+COM_Y.*Mb.*q1.*q3.*rk.*t4.*t5.*2.0+COM_Y.*Mb.*q2.*q4.*rk.*t3.*t4.*2.0+COM_Y.*Mb.*q1.*q3.*rk.*t4.*t6.*2.0+COM_Y.*Mb.*q2.*q4.*rk.*t3.*t5.*2.0+COM_Y.*Mb.*q1.*q3.*rk.*t5.*t6.*2.0+COM_Y.*Mb.*q2.*q4.*rk.*t3.*t6.*2.0+COM_Y.*Mb.*q2.*q4.*rk.*t4.*t5.*2.0+COM_Y.*Mb.*q2.*q4.*rk.*t4.*t6.*2.0+COM_Y.*Mb.*q2.*q4.*rk.*t5.*t6.*2.0).*2.0,Mb.*g.*rk.*t29.*(t37+t38+t39+t40+t41+t42+t43+t44+t45+t46+t47+t48+t49+t50+t51+t52+t53+t54+t55+t56+t57+t58+t59-Jk.*q2.*q3-Mb.*q2.*q3.*t2-Mk.*q2.*q3.*t2+COM_X.*Mb.*q1.*q2.*rk.*t7+COM_X.*Mb.*q1.*q2.*rk.*t8+COM_X.*Mb.*q1.*q2.*rk.*t9+COM_X.*Mb.*q1.*q2.*rk.*t10-COM_X.*Mb.*q3.*q4.*rk.*t7-COM_X.*Mb.*q3.*q4.*rk.*t8-COM_X.*Mb.*q3.*q4.*rk.*t9-COM_X.*Mb.*q3.*q4.*rk.*t10+COM_X.*Mb.*q1.*q2.*rk.*t3.*t4.*2.0+COM_X.*Mb.*q1.*q2.*rk.*t3.*t5.*2.0+COM_X.*Mb.*q1.*q2.*rk.*t3.*t6.*2.0+COM_X.*Mb.*q1.*q2.*rk.*t4.*t5.*2.0+COM_X.*Mb.*q1.*q2.*rk.*t4.*t6.*2.0+COM_X.*Mb.*q1.*q2.*rk.*t5.*t6.*2.0-COM_X.*Mb.*q3.*q4.*rk.*t3.*t4.*2.0-COM_X.*Mb.*q3.*q4.*rk.*t3.*t5.*2.0-COM_X.*Mb.*q3.*q4.*rk.*t3.*t6.*2.0-COM_X.*Mb.*q3.*q4.*rk.*t4.*t5.*2.0-COM_X.*Mb.*q3.*q4.*rk.*t4.*t6.*2.0-COM_X.*Mb.*q3.*q4.*rk.*t5.*t6.*2.0).*-2.0,-Mb.*g.*rk.*t29.*(-t60+t61-t62+t63-t64+t65-t66+t67+t68-t69-t70+t71-Jk.*t5+Jk.*t6-Mb.*t2.*t5+Mb.*t2.*t6-Mk.*t2.*t5+Mk.*t2.*t6-COM_Z.*Mb.*rk.*t3.*t8.*3.0-COM_Z.*Mb.*rk.*t4.*t7.*3.0+COM_Z.*Mb.*rk.*t3.*t9-COM_Z.*Mb.*rk.*t4.*t8-COM_Z.*Mb.*rk.*t5.*t7+COM_Z.*Mb.*rk.*t3.*t10-COM_Z.*Mb.*rk.*t5.*t8+COM_Z.*Mb.*rk.*t4.*t10-COM_Z.*Mb.*rk.*t6.*t8+COM_Z.*Mb.*rk.*t5.*t10.*3.0+COM_Z.*Mb.*rk.*t6.*t9.*3.0+COM_Z.*Mb.*rk.*t6.*t10+COM_X.*Mb.*q1.*q3.*rk.*t7.*2.0+COM_X.*Mb.*q1.*q3.*rk.*t8.*2.0+COM_X.*Mb.*q1.*q3.*rk.*t9.*2.0+COM_X.*Mb.*q2.*q4.*rk.*t7.*2.0+COM_X.*Mb.*q1.*q3.*rk.*t10.*2.0+COM_X.*Mb.*q2.*q4.*rk.*t8.*2.0+COM_X.*Mb.*q2.*q4.*rk.*t9.*2.0+COM_X.*Mb.*q2.*q4.*rk.*t10.*2.0-COM_Z.*Mb.*rk.*t3.*t4.*t6.*2.0+COM_Z.*Mb.*rk.*t4.*t5.*t6.*2.0+COM_X.*Mb.*q1.*q3.*rk.*t3.*t4.*4.0+COM_X.*Mb.*q1.*q3.*rk.*t3.*t5.*4.0+COM_X.*Mb.*q1.*q3.*rk.*t3.*t6.*4.0+COM_X.*Mb.*q1.*q3.*rk.*t4.*t5.*4.0+COM_X.*Mb.*q2.*q4.*rk.*t3.*t4.*4.0+COM_X.*Mb.*q1.*q3.*rk.*t4.*t6.*4.0+COM_X.*Mb.*q2.*q4.*rk.*t3.*t5.*4.0+COM_X.*Mb.*q1.*q3.*rk.*t5.*t6.*4.0+COM_X.*Mb.*q2.*q4.*rk.*t3.*t6.*4.0+COM_X.*Mb.*q2.*q4.*rk.*t4.*t5.*4.0+COM_X.*Mb.*q2.*q4.*rk.*t4.*t6.*4.0+COM_X.*Mb.*q2.*q4.*rk.*t5.*t6.*4.0)],[2,2]); 
+  f10 = (((t60 + t61) + t62) + t63) + t64;
+  x[0] = Mb * g * rk * t28 * (((((((((((((((((((((((((((((((((((((((((((((((f10
+    + t65) + t66) + t67) + t68) + t69) + t70) + t71) - Jk * t5) - Jk * t6) - Mb *
+    t2 * t5) - Mb * t2 * t6) - Mk * t2 * t5) - Mk * t2 * t6) - COM_Z * Mb * rk *
+    t3 * t8) + COM_Z * Mb * rk * t4 * t7) + COM_Z * Mb * rk * t3 * t9 * 3.0F) -
+    COM_Z * Mb * rk * t4 * t8) + COM_Z * Mb * rk * t5 * t7 * 3.0F) - COM_Z * Mb *
+    rk * t3 * t10) - COM_Z * Mb * rk * t5 * t8) - COM_Z * Mb * rk * t4 * t10 *
+    3.0F) - COM_Z * Mb * rk * t6 * t8 * 3.0F) - COM_Z * Mb * rk * t5 * t10) +
+    COM_Z * Mb * rk * t6 * t9) - COM_Z * Mb * rk * t6 * t10) + COM_Y * Mb * q1 *
+    q2 * rk * t7 * 2.0F) + COM_Y * Mb * q1 * q2 * rk * t8 * 2.0F) + COM_Y * Mb *
+    q1 * q2 * rk * t9 * 2.0F) + COM_Y * Mb * q1 * q2 * rk * t10 * 2.0F) - COM_Y *
+    Mb * q3 * q4 * rk * t7 * 2.0F) - COM_Y * Mb * q3 * q4 * rk * t8 * 2.0F) -
+    COM_Y * Mb * q3 * q4 * rk * t9 * 2.0F) - COM_Y * Mb * q3 * q4 * rk * t10 *
+    2.0F) - COM_Z * Mb * rk * t3 * t4 * t6 * 2.0F) - COM_Z * Mb * rk * t4 * t5 *
+    t6 * 2.0F) + COM_Y * Mb * q1 * q2 * rk * t3 * t4 * 4.0F) + COM_Y * Mb * q1 *
+    q2 * rk * t3 * t5 * 4.0F) + COM_Y * Mb * q1 * q2 * rk * t3 * t6 * 4.0F) +
+    COM_Y * Mb * q1 * q2 * rk * t4 * t5 * 4.0F) + COM_Y * Mb * q1 * q2 * rk * t4
+    * t6 * 4.0F) + COM_Y * Mb * q1 * q2 * rk * t5 * t6 * 4.0F) - COM_Y * Mb * q3
+    * q4 * rk * t3 * t4 * 4.0F) - COM_Y * Mb * q3 * q4 * rk * t3 * t5 * 4.0F) -
+    COM_Y * Mb * q3 * q4 * rk * t3 * t6 * 4.0F) - COM_Y * Mb * q3 * q4 * rk * t4
+    * t5 * 4.0F) - COM_Y * Mb * q3 * q4 * rk * t4 * t6 * 4.0F) - COM_Y * Mb * q3
+    * q4 * rk * t5 * t6 * 4.0F);
+  x[1] = Mb * g * rk * t28 * (((((((((((((((((((((((((((((((((((((((((((((t37 +
+    t38) + t39) + t40) - t41) + t42) - t43) + t44) - t45) - t46) + t47) + t48) -
+    t49) - t50) - t51) + t52) + t53) + t54) + t55) - t56) - t57) + t58) - t59) +
+    Jk * q2 * q3) + Mb * q2 * q3 * t2) + Mk * q2 * q3 * t2) + COM_Y * Mb * q1 *
+    q3 * rk * t7) + COM_Y * Mb * q1 * q3 * rk * t8) + COM_Y * Mb * q1 * q3 * rk *
+    t9) + COM_Y * Mb * q2 * q4 * rk * t7) + COM_Y * Mb * q1 * q3 * rk * t10) +
+    COM_Y * Mb * q2 * q4 * rk * t8) + COM_Y * Mb * q2 * q4 * rk * t9) + COM_Y *
+    Mb * q2 * q4 * rk * t10) + COM_Y * Mb * q1 * q3 * rk * t3 * t4 * 2.0F) +
+    COM_Y * Mb * q1 * q3 * rk * t3 * t5 * 2.0F) + COM_Y * Mb * q1 * q3 * rk * t3
+    * t6 * 2.0F) + COM_Y * Mb * q1 * q3 * rk * t4 * t5 * 2.0F) + COM_Y * Mb * q2
+    * q4 * rk * t3 * t4 * 2.0F) + COM_Y * Mb * q1 * q3 * rk * t4 * t6 * 2.0F) +
+    COM_Y * Mb * q2 * q4 * rk * t3 * t5 * 2.0F) + COM_Y * Mb * q1 * q3 * rk * t5
+    * t6 * 2.0F) + COM_Y * Mb * q2 * q4 * rk * t3 * t6 * 2.0F) + COM_Y * Mb * q2
+    * q4 * rk * t4 * t5 * 2.0F) + COM_Y * Mb * q2 * q4 * rk * t4 * t6 * 2.0F) +
+    COM_Y * Mb * q2 * q4 * rk * t5 * t6 * 2.0F) * 2.0F;
+  x[2] = Mb * g * rk * t28 * (((((((((((((((((((((((((((((((((((((((((((((t37 +
+    t38) + t39) + t40) + t41) + t42) + t43) + t44) + t45) + t46) + t47) + t48) +
+    t49) + t50) + t51) + t52) + t53) + t54) + t55) + t56) + t57) + t58) + t59) -
+    Jk * q2 * q3) - Mb * q2 * q3 * t2) - Mk * q2 * q3 * t2) + COM_X * Mb * q1 *
+    q2 * rk * t7) + COM_X * Mb * q1 * q2 * rk * t8) + COM_X * Mb * q1 * q2 * rk *
+    t9) + COM_X * Mb * q1 * q2 * rk * t10) - COM_X * Mb * q3 * q4 * rk * t7) -
+    COM_X * Mb * q3 * q4 * rk * t8) - COM_X * Mb * q3 * q4 * rk * t9) - COM_X *
+    Mb * q3 * q4 * rk * t10) + COM_X * Mb * q1 * q2 * rk * t3 * t4 * 2.0F) +
+    COM_X * Mb * q1 * q2 * rk * t3 * t5 * 2.0F) + COM_X * Mb * q1 * q2 * rk * t3
+    * t6 * 2.0F) + COM_X * Mb * q1 * q2 * rk * t4 * t5 * 2.0F) + COM_X * Mb * q1
+    * q2 * rk * t4 * t6 * 2.0F) + COM_X * Mb * q1 * q2 * rk * t5 * t6 * 2.0F) -
+    COM_X * Mb * q3 * q4 * rk * t3 * t4 * 2.0F) - COM_X * Mb * q3 * q4 * rk * t3
+    * t5 * 2.0F) - COM_X * Mb * q3 * q4 * rk * t3 * t6 * 2.0F) - COM_X * Mb * q3
+    * q4 * rk * t4 * t5 * 2.0F) - COM_X * Mb * q3 * q4 * rk * t4 * t6 * 2.0F) -
+    COM_X * Mb * q3 * q4 * rk * t5 * t6 * 2.0F) * -2.0F;
+  f10 = (((-t60 + t61) - t62) + t63) - t64;
+  x[3] = -Mb * g * rk * t28 * (((((((((((((((((((((((((((((((((((((((((((((((f10
+    + t65) - t66) + t67) + t68) - t69) - t70) + t71) - Jk * t5) + Jk * t6) - Mb *
+    t2 * t5) + Mb * t2 * t6) - Mk * t2 * t5) + Mk * t2 * t6) - COM_Z * Mb * rk *
+    t3 * t8 * 3.0F) - COM_Z * Mb * rk * t4 * t7 * 3.0F) + COM_Z * Mb * rk * t3 *
+    t9) - COM_Z * Mb * rk * t4 * t8) - COM_Z * Mb * rk * t5 * t7) + COM_Z * Mb *
+    rk * t3 * t10) - COM_Z * Mb * rk * t5 * t8) + COM_Z * Mb * rk * t4 * t10) -
+    COM_Z * Mb * rk * t6 * t8) + COM_Z * Mb * rk * t5 * t10 * 3.0F) + COM_Z * Mb
+    * rk * t6 * t9 * 3.0F) + COM_Z * Mb * rk * t6 * t10) + COM_X * Mb * q1 * q3 *
+    rk * t7 * 2.0F) + COM_X * Mb * q1 * q3 * rk * t8 * 2.0F) + COM_X * Mb * q1 *
+    q3 * rk * t9 * 2.0F) + COM_X * Mb * q2 * q4 * rk * t7 * 2.0F) + COM_X * Mb *
+    q1 * q3 * rk * t10 * 2.0F) + COM_X * Mb * q2 * q4 * rk * t8 * 2.0F) + COM_X *
+    Mb * q2 * q4 * rk * t9 * 2.0F) + COM_X * Mb * q2 * q4 * rk * t10 * 2.0F) -
+    COM_Z * Mb * rk * t3 * t4 * t6 * 2.0F) + COM_Z * Mb * rk * t4 * t5 * t6 *
+    2.0F) + COM_X * Mb * q1 * q3 * rk * t3 * t4 * 4.0F) + COM_X * Mb * q1 * q3 *
+    rk * t3 * t5 * 4.0F) + COM_X * Mb * q1 * q3 * rk * t3 * t6 * 4.0F) + COM_X *
+    Mb * q1 * q3 * rk * t4 * t5 * 4.0F) + COM_X * Mb * q2 * q4 * rk * t3 * t4 *
+    4.0F) + COM_X * Mb * q1 * q3 * rk * t4 * t6 * 4.0F) + COM_X * Mb * q2 * q4 *
+    rk * t3 * t5 * 4.0F) + COM_X * Mb * q1 * q3 * rk * t5 * t6 * 4.0F) + COM_X *
+    Mb * q2 * q4 * rk * t3 * t6 * 4.0F) + COM_X * Mb * q2 * q4 * rk * t4 * t5 *
+    4.0F) + COM_X * Mb * q2 * q4 * rk * t4 * t6 * 4.0F) + COM_X * Mb * q2 * q4 *
+    rk * t5 * t6 * 4.0F);
+  i4 = 0;
+  i5 = 0;
+  i6 = 0;
+  for (i7 = 0; i7 < 4; i7++) {
+    dAcceleration_dCOM[i5 + (i4 << 1)] = x[i6];
+    i4++;
+    if (i4 > 1) {
+      i4 = 0;
+      i5++;
+    }
+
+    i6++;
+  }
+}
+
+//
+// File trailer for SteadyStateAcceleration_dCOM.cpp
+//
+// [EOF]
+//
diff --git a/KugleFirmware/Libraries/Modules/Estimators/VelocityEKF/MATLABCoder/SteadyStateAcceleration_dCOM.h b/KugleFirmware/Libraries/Modules/Estimators/VelocityEKF/MATLABCoder/SteadyStateAcceleration_dCOM.h
new file mode 100644
index 0000000..8681c14
--- /dev/null
+++ b/KugleFirmware/Libraries/Modules/Estimators/VelocityEKF/MATLABCoder/SteadyStateAcceleration_dCOM.h
@@ -0,0 +1,30 @@
+//
+// Academic License - for use in teaching, academic research, and meeting
+// course requirements at degree granting institutions only.  Not for
+// government, commercial, or other organizational use.
+// File: SteadyStateAcceleration_dCOM.h
+//
+// MATLAB Coder version            : 4.0
+// C/C++ source code generated on  : 19-Nov-2018 11:57:59
+//
+#ifndef STEADYSTATEACCELERATION_DCOM_H
+#define STEADYSTATEACCELERATION_DCOM_H
+
+// Include Files
+#include <stddef.h>
+#include <stdlib.h>
+#include "rtwtypes.h"
+#include "VelocityEstimator_types.h"
+
+// Function Declarations
+extern void SteadyStateAcceleration_dCOM(float COM_X, float COM_Y, float COM_Z,
+  float Jk, float Mb, float Mk, float g, float q1, float q2, float q3, float q4,
+  float rk, float dAcceleration_dCOM[4]);
+
+#endif
+
+//
+// File trailer for SteadyStateAcceleration_dCOM.h
+//
+// [EOF]
+//
diff --git a/KugleFirmware/Libraries/Modules/Estimators/VelocityEKF/MATLABCoder/SteadyStateAcceleration_dq.cpp b/KugleFirmware/Libraries/Modules/Estimators/VelocityEKF/MATLABCoder/SteadyStateAcceleration_dq.cpp
new file mode 100644
index 0000000..15fe2b0
--- /dev/null
+++ b/KugleFirmware/Libraries/Modules/Estimators/VelocityEKF/MATLABCoder/SteadyStateAcceleration_dq.cpp
@@ -0,0 +1,920 @@
+//
+// Academic License - for use in teaching, academic research, and meeting
+// course requirements at degree granting institutions only.  Not for
+// government, commercial, or other organizational use.
+// File: SteadyStateAcceleration_dq.cpp
+//
+// MATLAB Coder version            : 4.0
+// C/C++ source code generated on  : 19-Nov-2018 11:57:59
+//
+
+// Include Files
+#include "rt_nonfinite.h"
+#include "VelocityEstimator.h"
+#include "SteadyStateAcceleration_dq.h"
+
+// Function Definitions
+
+//
+// function dAcceleration_dq = SteadyStateAcceleration_dq(COM_X,COM_Y,COM_Z,Jk,Mb,Mk,g,q1,q2,q3,q4,rk)
+// STEADYSTATEACCELERATION_DQ
+//     DACCELERATION_DQ = STEADYSTATEACCELERATION_DQ(COM_X,COM_Y,COM_Z,JK,MB,MK,G,Q1,Q2,Q3,Q4,RK)
+// Arguments    : float COM_X
+//                float COM_Y
+//                float COM_Z
+//                float Jk
+//                float Mb
+//                float Mk
+//                float g
+//                float q1
+//                float q2
+//                float q3
+//                float q4
+//                float rk
+//                float dAcceleration_dq[8]
+// Return Type  : void
+//
+__attribute__((optimize("O3"))) void SteadyStateAcceleration_dq(float COM_X, float COM_Y, float COM_Z, float Jk,
+  float Mb, float Mk, float g, float q1, float q2, float q3, float q4, float rk,
+  float dAcceleration_dq[8])
+{
+  float t2;
+  float t3;
+  float t4;
+  float t5;
+  float t6;
+  float t7;
+  float t8;
+  float t9;
+  float t10;
+  float t11;
+  float t12;
+  float t13;
+  float t31;
+  float t32;
+  float t40;
+  float t41;
+  float t42;
+  float t43;
+  float t44;
+  float t45;
+  float t46;
+  float t47;
+  float t48;
+  float t49;
+  float t50;
+  float t51;
+  float t52;
+  float t53;
+  float t54;
+  float t55;
+  float t56;
+  float t57;
+  float t58;
+  float t59;
+  float t60;
+  float t61;
+  float t62;
+  float t63;
+  float t64;
+  float t65;
+  float t67;
+  float t68;
+  float t69;
+  float t70;
+  float t71;
+  float t72;
+  float t73;
+  float t74;
+  float t75;
+  float t76;
+  float t77;
+  float t78;
+  float t79;
+  float t80;
+  float t81;
+  float t82;
+  float t83;
+  float t84;
+  float t85;
+  float t86;
+  float t87;
+  float t88;
+  float t89;
+  float t90;
+  float t91;
+  float t92;
+  float t93;
+  float t94;
+  float t95;
+  float t96;
+  float t97;
+  float t98;
+  float t99;
+  float t100;
+  float t101;
+  float t102;
+  float t103;
+  float t104;
+  float t105;
+  float t106;
+  float t107;
+  float t108;
+  float t109;
+  float t110;
+  float t111;
+  float t112;
+  float t113;
+  float t114;
+  float t115;
+  float t116;
+  float t117;
+  float t118;
+  float t119;
+  float t120;
+  float t121;
+  float t122;
+  float t123;
+  float t124;
+  float t125;
+  float t126;
+  float t127;
+  float t128;
+  float t129;
+  float t130;
+  float t131;
+  float t132;
+  float t133;
+  float t134;
+  float t135;
+  float t136;
+  float t137;
+  float t138;
+  float t139;
+  float t140;
+  float t141;
+  float t142;
+  float t143;
+  float t144;
+  float t145;
+  float t146;
+  float t147;
+  float f0;
+  float f1;
+  float f2;
+  float f3;
+  float f4;
+  float f5;
+  float f6;
+  float f7;
+  float f8;
+  float f9;
+  float x[8];
+  int i0;
+  int i1;
+  int i2;
+  int i3;
+
+  //     This function was generated by the Symbolic Math Toolbox version 8.1.
+  //     16-Nov-2018 12:04:26
+  // 'SteadyStateAcceleration_dq:8' t2 = rk.^2;
+  t2 = rk * rk;
+
+  // 'SteadyStateAcceleration_dq:9' t3 = q1.^2;
+  t3 = q1 * q1;
+
+  // 'SteadyStateAcceleration_dq:10' t4 = q2.^2;
+  t4 = q2 * q2;
+
+  // 'SteadyStateAcceleration_dq:11' t5 = q3.^2;
+  t5 = q3 * q3;
+
+  // 'SteadyStateAcceleration_dq:12' t6 = q4.^2;
+  t6 = q4 * q4;
+
+  // 'SteadyStateAcceleration_dq:13' t7 = t5.^2;
+  t7 = t5 * t5;
+
+  // 'SteadyStateAcceleration_dq:14' t8 = COM_X.^2;
+  t8 = COM_X * COM_X;
+
+  // 'SteadyStateAcceleration_dq:15' t9 = t3.^2;
+  t9 = t3 * t3;
+
+  // 'SteadyStateAcceleration_dq:16' t10 = t4.^2;
+  t10 = t4 * t4;
+
+  // 'SteadyStateAcceleration_dq:17' t11 = COM_Z.^2;
+  t11 = COM_Z * COM_Z;
+
+  // 'SteadyStateAcceleration_dq:18' t12 = t6.^2;
+  t12 = t6 * t6;
+
+  // 'SteadyStateAcceleration_dq:19' t13 = COM_Y.^2;
+  t13 = COM_Y * COM_Y;
+
+  // 'SteadyStateAcceleration_dq:20' t14 = Mb.*t2;
+  // 'SteadyStateAcceleration_dq:21' t15 = Mk.*t2;
+  // 'SteadyStateAcceleration_dq:22' t16 = COM_Z.*Mb.*rk.*t9;
+  // 'SteadyStateAcceleration_dq:23' t17 = COM_Z.*Mb.*rk.*t12;
+  // 'SteadyStateAcceleration_dq:24' t18 = COM_Z.*Mb.*rk.*t3.*t6.*2.0;
+  // 'SteadyStateAcceleration_dq:25' t19 = COM_Y.*Mb.*q1.*q2.*rk.*t4.*2.0;
+  // 'SteadyStateAcceleration_dq:26' t20 = COM_Y.*Mb.*q1.*q2.*rk.*t3.*2.0;
+  // 'SteadyStateAcceleration_dq:27' t21 = COM_X.*Mb.*q2.*q4.*rk.*t6.*2.0;
+  // 'SteadyStateAcceleration_dq:28' t22 = COM_X.*Mb.*q2.*q4.*rk.*t4.*2.0;
+  // 'SteadyStateAcceleration_dq:29' t23 = COM_Y.*Mb.*q3.*q4.*rk.*t6.*2.0;
+  // 'SteadyStateAcceleration_dq:30' t24 = COM_Y.*Mb.*q3.*q4.*rk.*t5.*2.0;
+  // 'SteadyStateAcceleration_dq:31' t25 = COM_X.*Mb.*q2.*q4.*rk.*t3.*2.0;
+  // 'SteadyStateAcceleration_dq:32' t26 = COM_Y.*Mb.*q1.*q2.*rk.*t5.*2.0;
+  // 'SteadyStateAcceleration_dq:33' t27 = COM_Y.*Mb.*q1.*q2.*rk.*t6.*2.0;
+  // 'SteadyStateAcceleration_dq:34' t28 = COM_X.*Mb.*q2.*q4.*rk.*t5.*2.0;
+  // 'SteadyStateAcceleration_dq:35' t29 = COM_Y.*Mb.*q3.*q4.*rk.*t3.*2.0;
+  // 'SteadyStateAcceleration_dq:36' t30 = COM_Y.*Mb.*q3.*q4.*rk.*t4.*2.0;
+  // 'SteadyStateAcceleration_dq:37' t33 = COM_Z.*Mb.*rk.*t10;
+  // 'SteadyStateAcceleration_dq:38' t34 = COM_Z.*Mb.*rk.*t7;
+  // 'SteadyStateAcceleration_dq:39' t35 = COM_Z.*Mb.*rk.*t4.*t5.*2.0;
+  // 'SteadyStateAcceleration_dq:40' t36 = COM_X.*Mb.*q1.*q3.*rk.*t5.*2.0;
+  // 'SteadyStateAcceleration_dq:41' t37 = COM_X.*Mb.*q1.*q3.*rk.*t3.*2.0;
+  // 'SteadyStateAcceleration_dq:42' t38 = COM_X.*Mb.*q1.*q3.*rk.*t4.*2.0;
+  // 'SteadyStateAcceleration_dq:43' t39 = COM_X.*Mb.*q1.*q3.*rk.*t6.*2.0;
+  // 'SteadyStateAcceleration_dq:44' t31 = Jk+t14+t15+t16+t17+t18+t19+t20+t21+t22+t23+t24+t25+t26+t27+t28+t29+t30-t33-t34-t35-t36-t37-t38-t39; 
+  t31 = (((((((((((((((((((((((Jk + Mb * t2) + Mk * t2) + COM_Z * Mb * rk * t9)
+    + COM_Z * Mb * rk * t12) + COM_Z * Mb * rk * t3 * t6 * 2.0F) + COM_Y * Mb *
+    q1 * q2 * rk * t4 * 2.0F) + COM_Y * Mb * q1 * q2 * rk * t3 * 2.0F) + COM_X *
+                        Mb * q2 * q4 * rk * t6 * 2.0F) + COM_X * Mb * q2 * q4 *
+                       rk * t4 * 2.0F) + COM_Y * Mb * q3 * q4 * rk * t6 * 2.0F)
+                     + COM_Y * Mb * q3 * q4 * rk * t5 * 2.0F) + COM_X * Mb * q2 *
+                    q4 * rk * t3 * 2.0F) + COM_Y * Mb * q1 * q2 * rk * t5 * 2.0F)
+                  + COM_Y * Mb * q1 * q2 * rk * t6 * 2.0F) + COM_X * Mb * q2 *
+                 q4 * rk * t5 * 2.0F) + COM_Y * Mb * q3 * q4 * rk * t3 * 2.0F) +
+               COM_Y * Mb * q3 * q4 * rk * t4 * 2.0F) - COM_Z * Mb * rk * t10) -
+             COM_Z * Mb * rk * t7) - COM_Z * Mb * rk * t4 * t5 * 2.0F) - COM_X *
+           Mb * q1 * q3 * rk * t5 * 2.0F) - COM_X * Mb * q1 * q3 * rk * t3 *
+          2.0F) - COM_X * Mb * q1 * q3 * rk * t4 * 2.0F) - COM_X * Mb * q1 * q3 *
+    rk * t6 * 2.0F;
+
+  // 'SteadyStateAcceleration_dq:45' t32 = 1.0./t31.^2;
+  t32 = 1.0F / (t31 * t31);
+
+  // 'SteadyStateAcceleration_dq:46' t40 = COM_Y.*Jk.*q1;
+  t40 = COM_Y * Jk * q1;
+
+  // 'SteadyStateAcceleration_dq:47' t41 = COM_X.*Jk.*q4;
+  t41 = COM_X * Jk * q4;
+
+  // 'SteadyStateAcceleration_dq:48' t42 = COM_Y.*Mb.*q1.*t2;
+  t42 = COM_Y * Mb * q1 * t2;
+
+  // 'SteadyStateAcceleration_dq:49' t43 = COM_X.*Mb.*q4.*t2;
+  t43 = COM_X * Mb * q4 * t2;
+
+  // 'SteadyStateAcceleration_dq:50' t44 = COM_Y.*Mk.*q1.*t2;
+  t44 = COM_Y * Mk * q1 * t2;
+
+  // 'SteadyStateAcceleration_dq:51' t45 = COM_X.*Mk.*q4.*t2;
+  t45 = COM_X * Mk * q4 * t2;
+
+  // 'SteadyStateAcceleration_dq:52' t46 = Mb.*q2.*rk.*t10.*t11;
+  t46 = Mb * q2 * rk * t10 * t11;
+
+  // 'SteadyStateAcceleration_dq:53' t47 = Mb.*q2.*rk.*t7.*t11;
+  t47 = Mb * q2 * rk * t7 * t11;
+
+  // 'SteadyStateAcceleration_dq:54' t48 = Mb.*q2.*rk.*t4.*t5.*t11.*2.0;
+  t48 = Mb * q2 * rk * t4 * t5 * t11 * 2.0F;
+
+  // 'SteadyStateAcceleration_dq:55' t49 = COM_Y.*COM_Z.*Mb.*q1.*rk.*t9;
+  t49 = COM_Y * COM_Z * Mb * q1 * rk * t9;
+
+  // 'SteadyStateAcceleration_dq:56' t50 = COM_X.*COM_Y.*Mb.*q3.*rk.*t4.*t5.*2.0; 
+  t50 = COM_X * COM_Y * Mb * q3 * rk * t4 * t5 * 2.0F;
+
+  // 'SteadyStateAcceleration_dq:57' t51 = COM_X.*COM_Z.*Mb.*q4.*rk.*t3.*t6.*2.0; 
+  t51 = COM_X * COM_Z * Mb * q4 * rk * t3 * t6 * 2.0F;
+
+  // 'SteadyStateAcceleration_dq:58' t52 = COM_Y.*COM_Z.*Mb.*q1.*rk.*t3.*t5.*2.0; 
+  t52 = COM_Y * COM_Z * Mb * q1 * rk * t3 * t5 * 2.0F;
+
+  // 'SteadyStateAcceleration_dq:59' t53 = Mb.*q1.*q3.*q4.*rk.*t6.*t11.*4.0;
+  t53 = Mb * q1 * q3 * q4 * rk * t6 * t11 * 4.0F;
+
+  // 'SteadyStateAcceleration_dq:60' t54 = Mb.*q1.*q3.*q4.*rk.*t3.*t11.*4.0;
+  t54 = Mb * q1 * q3 * q4 * rk * t3 * t11 * 4.0F;
+
+  // 'SteadyStateAcceleration_dq:61' t55 = Mb.*q2.*rk.*t3.*t6.*t11.*2.0;
+  t55 = Mb * q2 * rk * t3 * t6 * t11 * 2.0F;
+
+  // 'SteadyStateAcceleration_dq:62' t56 = COM_X.*COM_Y.*Mb.*q3.*rk.*t10.*3.0;
+  t56 = COM_X * COM_Y * Mb * q3 * rk * t10 * 3.0F;
+
+  // 'SteadyStateAcceleration_dq:63' t57 = COM_X.*COM_Z.*Mb.*q4.*rk.*t9.*3.0;
+  t57 = COM_X * COM_Z * Mb * q4 * rk * t9 * 3.0F;
+
+  // 'SteadyStateAcceleration_dq:64' t58 = COM_Y.*COM_Z.*Mb.*q1.*rk.*t7;
+  t58 = COM_Y * COM_Z * Mb * q1 * rk * t7;
+
+  // 'SteadyStateAcceleration_dq:65' t59 = COM_X.*COM_Y.*Mb.*q1.*q2.*q4.*rk.*t4.*4.0; 
+  t59 = COM_X * COM_Y * Mb * q1 * q2 * q4 * rk * t4 * 4.0F;
+
+  // 'SteadyStateAcceleration_dq:66' t60 = COM_X.*COM_Y.*Mb.*q1.*q2.*q4.*rk.*t3.*4.0; 
+  t60 = COM_X * COM_Y * Mb * q1 * q2 * q4 * rk * t3 * 4.0F;
+
+  // 'SteadyStateAcceleration_dq:67' t61 = COM_X.*COM_Y.*Mb.*q3.*rk.*t3.*t6.*6.0; 
+  t61 = COM_X * COM_Y * Mb * q3 * rk * t3 * t6 * 6.0F;
+
+  // 'SteadyStateAcceleration_dq:68' t62 = COM_X.*COM_Z.*Mb.*q4.*rk.*t3.*t4.*2.0; 
+  t62 = COM_X * COM_Z * Mb * q4 * rk * t3 * t4 * 2.0F;
+
+  // 'SteadyStateAcceleration_dq:69' t63 = COM_Y.*COM_Z.*Mb.*q1.*rk.*t4.*t6.*2.0; 
+  t63 = COM_Y * COM_Z * Mb * q1 * rk * t4 * t6 * 2.0F;
+
+  // 'SteadyStateAcceleration_dq:70' t64 = COM_Y.*COM_Z.*Mb.*q1.*rk.*t5.*t6.*6.0; 
+  t64 = COM_Y * COM_Z * Mb * q1 * rk * t5 * t6 * 6.0F;
+
+  // 'SteadyStateAcceleration_dq:71' t65 = Mb.*q1.*q3.*q4.*rk.*t4.*t13.*4.0;
+  t65 = Mb * q1 * q3 * q4 * rk * t4 * t13 * 4.0F;
+
+  // 'SteadyStateAcceleration_dq:72' t66 = COM_Y.*Jk.*q2;
+  t31 = COM_Y * Jk * q2;
+
+  // 'SteadyStateAcceleration_dq:73' t67 = COM_Z.*Jk.*q1;
+  t67 = COM_Z * Jk * q1;
+
+  // 'SteadyStateAcceleration_dq:74' t68 = COM_Y.*Mb.*q2.*t2;
+  t68 = COM_Y * Mb * q2 * t2;
+
+  // 'SteadyStateAcceleration_dq:75' t69 = COM_Z.*Mb.*q1.*t2;
+  t69 = COM_Z * Mb * q1 * t2;
+
+  // 'SteadyStateAcceleration_dq:76' t70 = COM_Y.*Mk.*q2.*t2;
+  t70 = COM_Y * Mk * q2 * t2;
+
+  // 'SteadyStateAcceleration_dq:77' t71 = COM_Z.*Mk.*q1.*t2;
+  t71 = COM_Z * Mk * q1 * t2;
+
+  // 'SteadyStateAcceleration_dq:78' t72 = Mb.*q1.*rk.*t9.*t11;
+  t72 = Mb * q1 * rk * t9 * t11;
+
+  // 'SteadyStateAcceleration_dq:79' t73 = Mb.*q1.*rk.*t11.*t12;
+  t73 = Mb * q1 * rk * t11 * t12;
+
+  // 'SteadyStateAcceleration_dq:80' t74 = Mb.*q1.*rk.*t3.*t6.*t11.*2.0;
+  t74 = Mb * q1 * rk * t3 * t6 * t11 * 2.0F;
+
+  // 'SteadyStateAcceleration_dq:81' t75 = COM_X.*COM_Y.*Mb.*q4.*rk.*t12;
+  t75 = COM_X * COM_Y * Mb * q4 * rk * t12;
+
+  // 'SteadyStateAcceleration_dq:82' t76 = COM_X.*COM_Y.*Mb.*q4.*rk.*t4.*t6.*2.0; 
+  t76 = COM_X * COM_Y * Mb * q4 * rk * t4 * t6 * 2.0F;
+
+  // 'SteadyStateAcceleration_dq:83' t77 = COM_X.*COM_Z.*Mb.*q3.*rk.*t4.*t5.*2.0; 
+  t77 = COM_X * COM_Z * Mb * q3 * rk * t4 * t5 * 2.0F;
+
+  // 'SteadyStateAcceleration_dq:84' t78 = COM_X.*COM_Y.*Mb.*q4.*rk.*t5.*t6.*2.0; 
+  t78 = COM_X * COM_Y * Mb * q4 * rk * t5 * t6 * 2.0F;
+
+  // 'SteadyStateAcceleration_dq:85' t79 = COM_Y.*COM_Z.*Mb.*q2.*rk.*t4.*t5.*2.0; 
+  t79 = COM_Y * COM_Z * Mb * q2 * rk * t4 * t5 * 2.0F;
+
+  // 'SteadyStateAcceleration_dq:86' t80 = Mb.*q2.*q3.*q4.*rk.*t5.*t11.*4.0;
+  t80 = Mb * q2 * q3 * q4 * rk * t5 * t11 * 4.0F;
+
+  // 'SteadyStateAcceleration_dq:87' t81 = Mb.*q2.*q3.*q4.*rk.*t4.*t11.*4.0;
+  t81 = Mb * q2 * q3 * q4 * rk * t4 * t11 * 4.0F;
+
+  // 'SteadyStateAcceleration_dq:88' t82 = Mb.*q1.*rk.*t4.*t5.*t11.*2.0;
+  t82 = Mb * q1 * rk * t4 * t5 * t11 * 2.0F;
+
+  // 'SteadyStateAcceleration_dq:89' t83 = COM_X.*COM_Y.*Mb.*q4.*rk.*t10;
+  t83 = COM_X * COM_Y * Mb * q4 * rk * t10;
+
+  // 'SteadyStateAcceleration_dq:90' t84 = COM_X.*COM_Z.*Mb.*q3.*rk.*t10.*3.0;
+  t84 = COM_X * COM_Z * Mb * q3 * rk * t10 * 3.0F;
+
+  // 'SteadyStateAcceleration_dq:91' t85 = COM_X.*COM_Y.*Mb.*q4.*rk.*t7;
+  t85 = COM_X * COM_Y * Mb * q4 * rk * t7;
+
+  // 'SteadyStateAcceleration_dq:92' t86 = COM_Y.*COM_Z.*Mb.*q2.*rk.*t7.*3.0;
+  t86 = COM_Y * COM_Z * Mb * q2 * rk * t7 * 3.0F;
+
+  // 'SteadyStateAcceleration_dq:93' t87 = COM_X.*COM_Y.*Mb.*q1.*q2.*q3.*rk.*t5.*4.0; 
+  t87 = COM_X * COM_Y * Mb * q1 * q2 * q3 * rk * t5 * 4.0F;
+
+  // 'SteadyStateAcceleration_dq:94' t88 = COM_X.*COM_Z.*Mb.*q3.*rk.*t3.*t4.*2.0; 
+  t88 = COM_X * COM_Z * Mb * q3 * rk * t3 * t4 * 2.0F;
+
+  // 'SteadyStateAcceleration_dq:95' t89 = COM_Y.*COM_Z.*Mb.*q2.*rk.*t3.*t6.*2.0; 
+  t89 = COM_Y * COM_Z * Mb * q2 * rk * t3 * t6 * 2.0F;
+
+  // 'SteadyStateAcceleration_dq:96' t90 = Mb.*q2.*q3.*q4.*rk.*t3.*t13.*4.0;
+  t90 = Mb * q2 * q3 * q4 * rk * t3 * t13 * 4.0F;
+
+  // 'SteadyStateAcceleration_dq:97' t91 = COM_X.*COM_Y.*Mb.*q1.*q2.*q3.*rk.*t6.*4.0; 
+  t91 = COM_X * COM_Y * Mb * q1 * q2 * q3 * rk * t6 * 4.0F;
+
+  // 'SteadyStateAcceleration_dq:98' t92 = COM_X.*Jk.*q2;
+  t92 = COM_X * Jk * q2;
+
+  // 'SteadyStateAcceleration_dq:99' t93 = COM_Y.*Jk.*q3;
+  t93 = COM_Y * Jk * q3;
+
+  // 'SteadyStateAcceleration_dq:100' t94 = COM_Z.*Jk.*q4;
+  t94 = COM_Z * Jk * q4;
+
+  // 'SteadyStateAcceleration_dq:101' t95 = COM_X.*Mb.*q2.*t2;
+  t95 = COM_X * Mb * q2 * t2;
+
+  // 'SteadyStateAcceleration_dq:102' t96 = COM_Y.*Mb.*q3.*t2;
+  t96 = COM_Y * Mb * q3 * t2;
+
+  // 'SteadyStateAcceleration_dq:103' t97 = COM_Z.*Mb.*q4.*t2;
+  t97 = COM_Z * Mb * q4 * t2;
+
+  // 'SteadyStateAcceleration_dq:104' t98 = COM_X.*Mk.*q2.*t2;
+  t98 = COM_X * Mk * q2 * t2;
+
+  // 'SteadyStateAcceleration_dq:105' t99 = COM_Y.*Mk.*q3.*t2;
+  t99 = COM_Y * Mk * q3 * t2;
+
+  // 'SteadyStateAcceleration_dq:106' t100 = COM_Z.*Mk.*q4.*t2;
+  t100 = COM_Z * Mk * q4 * t2;
+
+  // 'SteadyStateAcceleration_dq:107' t101 = Mb.*q4.*rk.*t11.*t12;
+  t101 = Mb * q4 * rk * t11 * t12;
+
+  // 'SteadyStateAcceleration_dq:108' t102 = Mb.*q4.*rk.*t9.*t11;
+  t102 = Mb * q4 * rk * t9 * t11;
+
+  // 'SteadyStateAcceleration_dq:109' t103 = Mb.*q4.*rk.*t3.*t6.*t11.*2.0;
+  t103 = Mb * q4 * rk * t3 * t6 * t11 * 2.0F;
+
+  // 'SteadyStateAcceleration_dq:110' t104 = COM_X.*COM_Z.*Mb.*q2.*rk.*t10;
+  t104 = COM_X * COM_Z * Mb * q2 * rk * t10;
+
+  // 'SteadyStateAcceleration_dq:111' t105 = COM_X.*COM_Z.*Mb.*q2.*rk.*t3.*t4.*2.0; 
+  t105 = COM_X * COM_Z * Mb * q2 * rk * t3 * t4 * 2.0F;
+
+  // 'SteadyStateAcceleration_dq:112' t106 = COM_X.*COM_Y.*Mb.*q1.*rk.*t3.*t6.*2.0; 
+  t106 = COM_X * COM_Y * Mb * q1 * rk * t3 * t6 * 2.0F;
+
+  // 'SteadyStateAcceleration_dq:113' t107 = COM_Y.*COM_Z.*Mb.*q3.*rk.*t4.*t5.*2.0; 
+  t107 = COM_Y * COM_Z * Mb * q3 * rk * t4 * t5 * 2.0F;
+
+  // 'SteadyStateAcceleration_dq:114' t108 = Mb.*q1.*q2.*q3.*rk.*t5.*t11.*4.0;
+  t108 = Mb * q1 * q2 * q3 * rk * t5 * t11 * 4.0F;
+
+  // 'SteadyStateAcceleration_dq:115' t109 = Mb.*q1.*q2.*q3.*rk.*t4.*t11.*4.0;
+  t109 = Mb * q1 * q2 * q3 * rk * t4 * t11 * 4.0F;
+
+  // 'SteadyStateAcceleration_dq:116' t110 = Mb.*q4.*rk.*t4.*t5.*t11.*2.0;
+  t110 = Mb * q4 * rk * t4 * t5 * t11 * 2.0F;
+
+  // 'SteadyStateAcceleration_dq:117' t111 = COM_X.*COM_Z.*Mb.*q2.*rk.*t9;
+  t111 = COM_X * COM_Z * Mb * q2 * rk * t9;
+
+  // 'SteadyStateAcceleration_dq:118' t112 = COM_X.*COM_Y.*Mb.*q1.*rk.*t12.*3.0; 
+  t112 = COM_X * COM_Y * Mb * q1 * rk * t12 * 3.0F;
+
+  // 'SteadyStateAcceleration_dq:119' t113 = COM_Y.*COM_Z.*Mb.*q3.*rk.*t10.*3.0; 
+  t113 = COM_Y * COM_Z * Mb * q3 * rk * t10 * 3.0F;
+
+  // 'SteadyStateAcceleration_dq:120' t114 = COM_X.*COM_Y.*Mb.*q2.*q3.*q4.*rk.*t6.*4.0; 
+  t114 = COM_X * COM_Y * Mb * q2 * q3 * q4 * rk * t6 * 4.0F;
+
+  // 'SteadyStateAcceleration_dq:121' t115 = COM_X.*COM_Y.*Mb.*q2.*q3.*q4.*rk.*t5.*4.0; 
+  t115 = COM_X * COM_Y * Mb * q2 * q3 * q4 * rk * t5 * 4.0F;
+
+  // 'SteadyStateAcceleration_dq:122' t116 = COM_X.*COM_Y.*Mb.*q1.*rk.*t4.*t5.*6.0; 
+  t116 = COM_X * COM_Y * Mb * q1 * rk * t4 * t5 * 6.0F;
+
+  // 'SteadyStateAcceleration_dq:123' t117 = COM_X.*COM_Z.*Mb.*q2.*rk.*t3.*t5.*6.0; 
+  t117 = COM_X * COM_Z * Mb * q2 * rk * t3 * t5 * 6.0F;
+
+  // 'SteadyStateAcceleration_dq:124' t118 = COM_X.*COM_Z.*Mb.*q2.*rk.*t3.*t6.*2.0; 
+  t118 = COM_X * COM_Z * Mb * q2 * rk * t3 * t6 * 2.0F;
+
+  // 'SteadyStateAcceleration_dq:125' t119 = COM_Y.*COM_Z.*Mb.*q3.*rk.*t3.*t6.*2.0; 
+  t119 = COM_Y * COM_Z * Mb * q3 * rk * t3 * t6 * 2.0F;
+
+  // 'SteadyStateAcceleration_dq:126' t120 = Mb.*q1.*q2.*q3.*rk.*t6.*t13.*4.0;
+  t120 = Mb * q1 * q2 * q3 * rk * t6 * t13 * 4.0F;
+
+  // 'SteadyStateAcceleration_dq:127' t121 = COM_Y.*Jk.*q4;
+  t121 = COM_Y * Jk * q4;
+
+  // 'SteadyStateAcceleration_dq:128' t122 = COM_Y.*Mb.*q4.*t2;
+  t122 = COM_Y * Mb * q4 * t2;
+
+  // 'SteadyStateAcceleration_dq:129' t123 = COM_Y.*Mk.*q4.*t2;
+  t123 = COM_Y * Mk * q4 * t2;
+
+  // 'SteadyStateAcceleration_dq:130' t124 = Mb.*q3.*rk.*t7.*t11;
+  t124 = Mb * q3 * rk * t7 * t11;
+
+  // 'SteadyStateAcceleration_dq:131' t125 = Mb.*q3.*rk.*t10.*t11;
+  t125 = Mb * q3 * rk * t10 * t11;
+
+  // 'SteadyStateAcceleration_dq:132' t126 = Mb.*q3.*rk.*t4.*t5.*t11.*2.0;
+  t126 = Mb * q3 * rk * t4 * t5 * t11 * 2.0F;
+
+  // 'SteadyStateAcceleration_dq:133' t127 = COM_X.*COM_Y.*Mb.*q2.*rk.*t10;
+  t127 = COM_X * COM_Y * Mb * q2 * rk * t10;
+
+  // 'SteadyStateAcceleration_dq:134' t128 = COM_X.*COM_Z.*Mb.*q1.*rk.*t9;
+  t128 = COM_X * COM_Z * Mb * q1 * rk * t9;
+
+  // 'SteadyStateAcceleration_dq:135' t129 = COM_Y.*COM_Z.*Mb.*q4.*rk.*t12;
+  t129 = COM_Y * COM_Z * Mb * q4 * rk * t12;
+
+  // 'SteadyStateAcceleration_dq:136' t130 = COM_X.*COM_Y.*Mb.*q2.*rk.*t3.*t4.*2.0; 
+  t130 = COM_X * COM_Y * Mb * q2 * rk * t3 * t4 * 2.0F;
+
+  // 'SteadyStateAcceleration_dq:137' t131 = COM_X.*COM_Z.*Mb.*q1.*rk.*t3.*t4.*2.0; 
+  t131 = COM_X * COM_Z * Mb * q1 * rk * t3 * t4 * 2.0F;
+
+  // 'SteadyStateAcceleration_dq:138' t132 = COM_X.*COM_Y.*Mb.*q2.*rk.*t4.*t6.*2.0; 
+  t132 = COM_X * COM_Y * Mb * q2 * rk * t4 * t6 * 2.0F;
+
+  // 'SteadyStateAcceleration_dq:139' t133 = COM_Y.*COM_Z.*Mb.*q4.*rk.*t4.*t6.*2.0; 
+  t133 = COM_Y * COM_Z * Mb * q4 * rk * t4 * t6 * 2.0F;
+
+  // 'SteadyStateAcceleration_dq:140' t134 = Mb.*q1.*q2.*q4.*rk.*t6.*t11.*4.0;
+  t134 = Mb * q1 * q2 * q4 * rk * t6 * t11 * 4.0F;
+
+  // 'SteadyStateAcceleration_dq:141' t135 = Mb.*q1.*q2.*q4.*rk.*t3.*t11.*4.0;
+  t135 = Mb * q1 * q2 * q4 * rk * t3 * t11 * 4.0F;
+
+  // 'SteadyStateAcceleration_dq:142' t136 = Mb.*q3.*rk.*t3.*t6.*t11.*2.0;
+  t136 = Mb * q3 * rk * t3 * t6 * t11 * 2.0F;
+
+  // 'SteadyStateAcceleration_dq:143' t137 = COM_X.*COM_Y.*Mb.*q2.*rk.*t9;
+  t137 = COM_X * COM_Y * Mb * q2 * rk * t9;
+
+  // 'SteadyStateAcceleration_dq:144' t138 = COM_X.*COM_Z.*Mb.*q1.*rk.*t10;
+  t138 = COM_X * COM_Z * Mb * q1 * rk * t10;
+
+  // 'SteadyStateAcceleration_dq:145' t139 = COM_X.*COM_Y.*Mb.*q2.*rk.*t12;
+  t139 = COM_X * COM_Y * Mb * q2 * rk * t12;
+
+  // 'SteadyStateAcceleration_dq:146' t140 = COM_Y.*COM_Z.*Mb.*q4.*rk.*t10;
+  t140 = COM_Y * COM_Z * Mb * q4 * rk * t10;
+
+  // 'SteadyStateAcceleration_dq:147' t141 = COM_X.*COM_Y.*Mb.*q1.*q3.*q4.*rk.*t3.*4.0; 
+  t141 = COM_X * COM_Y * Mb * q1 * q3 * q4 * rk * t3 * 4.0F;
+
+  // 'SteadyStateAcceleration_dq:148' t142 = COM_X.*COM_Z.*Mb.*q1.*rk.*t4.*t5.*2.0; 
+  t142 = COM_X * COM_Z * Mb * q1 * rk * t4 * t5 * 2.0F;
+
+  // 'SteadyStateAcceleration_dq:149' t143 = COM_X.*COM_Z.*Mb.*q1.*rk.*t4.*t6.*6.0; 
+  t143 = COM_X * COM_Z * Mb * q1 * rk * t4 * t6 * 6.0F;
+
+  // 'SteadyStateAcceleration_dq:150' t144 = COM_Y.*COM_Z.*Mb.*q4.*rk.*t3.*t4.*6.0; 
+  t144 = COM_Y * COM_Z * Mb * q4 * rk * t3 * t4 * 6.0F;
+
+  // 'SteadyStateAcceleration_dq:151' t145 = COM_Y.*COM_Z.*Mb.*q4.*rk.*t3.*t5.*2.0; 
+  t145 = COM_Y * COM_Z * Mb * q4 * rk * t3 * t5 * 2.0F;
+
+  // 'SteadyStateAcceleration_dq:152' t146 = Mb.*q1.*q2.*q4.*rk.*t5.*t13.*4.0;
+  t146 = Mb * q1 * q2 * q4 * rk * t5 * t13 * 4.0F;
+
+  // 'SteadyStateAcceleration_dq:153' t147 = COM_X.*COM_Y.*Mb.*q1.*q3.*q4.*rk.*t4.*4.0; 
+  t147 = COM_X * COM_Y * Mb * q1 * q3 * q4 * rk * t4 * 4.0F;
+
+  // 'SteadyStateAcceleration_dq:154' dAcceleration_dq = reshape([Mb.*g.*rk.*t32.*(t121+t122+t123+t124+t125+t126+t127+t128+t129+t130+t131+t132+t133+t134+t135+t136+t137+t138+t139+t140+t141+t142+t143+t144+t145+t146+t147-COM_X.*Jk.*q1-COM_Z.*Jk.*q3-COM_X.*Mb.*q1.*t2-COM_Z.*Mb.*q3.*t2-COM_X.*Mk.*q1.*t2-COM_Z.*Mk.*q3.*t2+Mb.*q3.*rk.*t7.*t8-Mb.*q3.*rk.*t8.*t9-Mb.*q3.*rk.*t8.*t10+Mb.*q3.*rk.*t8.*t12+Mb.*q3.*rk.*t9.*t11.*3.0+Mb.*q3.*rk.*t10.*t13.*2.0-Mb.*q3.*rk.*t11.*t12+Mb.*q3.*rk.*t12.*t13.*2.0-COM_X.*COM_Y.*Mb.*q2.*rk.*t7.*3.0+COM_X.*COM_Z.*Mb.*q1.*rk.*t7-COM_Y.*COM_Z.*Mb.*q4.*rk.*t7.*3.0-COM_X.*COM_Z.*Mb.*q1.*rk.*t12.*3.0-COM_Y.*COM_Z.*Mb.*q4.*rk.*t9.*3.0-Mb.*q3.*rk.*t3.*t4.*t8.*2.0+Mb.*q3.*rk.*t3.*t5.*t8.*4.0+Mb.*q3.*rk.*t3.*t6.*t8.*4.0+Mb.*q3.*rk.*t5.*t6.*t8.*2.0+Mb.*q3.*rk.*t3.*t4.*t13.*6.0-Mb.*q3.*rk.*t3.*t6.*t13.*2.0+Mb.*q3.*rk.*t4.*t5.*t13.*2.0+Mb.*q3.*rk.*t4.*t6.*t13.*4.0+Mb.*q3.*rk.*t5.*t6.*t13.*2.0-Mb.*q1.*q2.*q4.*rk.*t5.*t8.*4.0-Mb.*q1.*q2.*q4.*rk.*t6.*t8.*4.0-Mb.*q1.*q2.*q4.*rk.*t3.*t13.*4.0-COM_X.*COM_Y.*Mb.*q2.*rk.*t3.*t5.*1.0e1-COM_X.*COM_Z.*Mb.*q1.*rk.*t3.*t5.*6.0-COM_X.*COM_Y.*Mb.*q2.*rk.*t3.*t6.*6.0-COM_X.*COM_Y.*Mb.*q2.*rk.*t4.*t5.*2.0-COM_X.*COM_Z.*Mb.*q1.*rk.*t3.*t6.*2.0-COM_X.*COM_Y.*Mb.*q2.*rk.*t5.*t6.*2.0-COM_X.*COM_Z.*Mb.*q1.*rk.*t5.*t6.*2.0-COM_Y.*COM_Z.*Mb.*q4.*rk.*t3.*t6.*2.0-COM_Y.*COM_Z.*Mb.*q4.*rk.*t4.*t5.*2.0-COM_Y.*COM_Z.*Mb.*q4.*rk.*t5.*t6.*2.0+COM_Y.*COM_Z.*Mb.*q1.*q2.*q3.*rk.*t3.*8.0-COM_X.*COM_Y.*Mb.*q1.*q3.*q4.*rk.*t5.*4.0-COM_X.*COM_Z.*Mb.*q2.*q3.*q4.*rk.*t3.*4.0-COM_X.*COM_Y.*Mb.*q1.*q3.*q4.*rk.*t6.*4.0-COM_X.*COM_Z.*Mb.*q2.*q3.*q4.*rk.*t4.*4.0+COM_Y.*COM_Z.*Mb.*q1.*q2.*q3.*rk.*t6.*8.0-COM_X.*COM_Z.*Mb.*q2.*q3.*q4.*rk.*t5.*4.0-COM_X.*COM_Z.*Mb.*q2.*q3.*q4.*rk.*t6.*4.0).*-2.0,Mb.*g.*rk.*t32.*(t40+t41+t42+t43+t44+t45+t46+t47+t48-t49-t50-t51-t52-t53-t54+t55-t56-t57-t58+t59-t60-t61+t62+t63-t64+t65-COM_Z.*Jk.*q2-COM_Z.*Mb.*q2.*t2-COM_Z.*Mk.*q2.*t2+Mb.*q2.*rk.*t7.*t8.*2.0-Mb.*q2.*rk.*t7.*t13+Mb.*q2.*rk.*t8.*t12.*2.0+Mb.*q2.*rk.*t9.*t11.*3.0-Mb.*q2.*rk.*t9.*t13+Mb.*q2.*rk.*t10.*t13-Mb.*q2.*rk.*t11.*t12+Mb.*q2.*rk.*t12.*t13+COM_X.*COM_Y.*Mb.*q3.*rk.*t7+COM_X.*COM_Y.*Mb.*q3.*rk.*t9+COM_X.*COM_Z.*Mb.*q4.*rk.*t7-COM_Y.*COM_Z.*Mb.*q1.*rk.*t10+COM_X.*COM_Y.*Mb.*q3.*rk.*t12-COM_X.*COM_Z.*Mb.*q4.*rk.*t10.*3.0+COM_Y.*COM_Z.*Mb.*q1.*rk.*t12.*3.0+COM_X.*COM_Z.*Mb.*q4.*rk.*t12+Mb.*q2.*rk.*t3.*t5.*t8.*6.0-Mb.*q2.*rk.*t3.*t6.*t8.*2.0+Mb.*q2.*rk.*t4.*t5.*t8.*2.0+Mb.*q2.*rk.*t4.*t6.*t8.*2.0+Mb.*q2.*rk.*t5.*t6.*t8.*4.0+Mb.*q2.*rk.*t3.*t4.*t13.*4.0-Mb.*q2.*rk.*t3.*t5.*t13.*2.0+Mb.*q2.*rk.*t3.*t6.*t13.*4.0+Mb.*q2.*rk.*t4.*t6.*t13.*2.0+Mb.*q1.*q3.*q4.*rk.*t3.*t8.*4.0-Mb.*q1.*q3.*q4.*rk.*t4.*t8.*4.0+Mb.*q1.*q3.*q4.*rk.*t6.*t13.*4.0-COM_X.*COM_Y.*Mb.*q3.*rk.*t3.*t4.*1.0e1+COM_Y.*COM_Z.*Mb.*q1.*rk.*t3.*t4.*6.0+COM_X.*COM_Y.*Mb.*q3.*rk.*t3.*t5.*2.0+COM_Y.*COM_Z.*Mb.*q1.*rk.*t3.*t6.*2.0-COM_Y.*COM_Z.*Mb.*q1.*rk.*t4.*t5.*2.0-COM_X.*COM_Y.*Mb.*q3.*rk.*t4.*t6.*2.0+COM_X.*COM_Z.*Mb.*q4.*rk.*t3.*t5.*6.0+COM_X.*COM_Y.*Mb.*q3.*rk.*t5.*t6.*2.0-COM_X.*COM_Z.*Mb.*q4.*rk.*t4.*t5.*2.0-COM_X.*COM_Z.*Mb.*q4.*rk.*t4.*t6.*2.0+COM_X.*COM_Z.*Mb.*q4.*rk.*t5.*t6.*2.0-COM_X.*COM_Z.*Mb.*q1.*q2.*q3.*rk.*t3.*8.0-COM_X.*COM_Y.*Mb.*q1.*q2.*q4.*rk.*t5.*4.0+COM_X.*COM_Y.*Mb.*q1.*q2.*q4.*rk.*t6.*4.0-COM_X.*COM_Z.*Mb.*q1.*q2.*q3.*rk.*t6.*8.0-COM_Y.*COM_Z.*Mb.*q2.*q3.*q4.*rk.*t3.*4.0-COM_Y.*COM_Z.*Mb.*q2.*q3.*q4.*rk.*t4.*4.0-COM_Y.*COM_Z.*Mb.*q2.*q3.*q4.*rk.*t5.*4.0-COM_Y.*COM_Z.*Mb.*q2.*q3.*q4.*rk.*t6.*4.0).*2.0,Mb.*g.*rk.*t32.*(t92+t93+t94+t95+t96+t97+t98+t99+t100+t101+t102+t103+t104+t105+t106+t107+t108+t109+t110+t111+t112+t113+t114+t115+t116+t117+t118+t119+t120+Mb.*q4.*rk.*t7.*t8-Mb.*q4.*rk.*t8.*t9-Mb.*q4.*rk.*t7.*t11-Mb.*q4.*rk.*t8.*t10+Mb.*q4.*rk.*t7.*t13.*2.0+Mb.*q4.*rk.*t8.*t12+Mb.*q4.*rk.*t10.*t11.*3.0+Mb.*q4.*rk.*t9.*t13.*2.0-COM_X.*COM_Y.*Mb.*q1.*rk.*t7-COM_X.*COM_Y.*Mb.*q1.*rk.*t9-COM_X.*COM_Z.*Mb.*q2.*rk.*t7.*3.0-COM_X.*COM_Y.*Mb.*q1.*rk.*t10-COM_Y.*COM_Z.*Mb.*q3.*rk.*t7-COM_Y.*COM_Z.*Mb.*q3.*rk.*t9+COM_X.*COM_Z.*Mb.*q2.*rk.*t12+COM_Y.*COM_Z.*Mb.*q3.*rk.*t12.*3.0-Mb.*q4.*rk.*t3.*t4.*t8.*2.0+Mb.*q4.*rk.*t4.*t5.*t8.*4.0+Mb.*q4.*rk.*t4.*t6.*t8.*4.0+Mb.*q4.*rk.*t5.*t6.*t8.*2.0+Mb.*q4.*rk.*t3.*t4.*t13.*6.0+Mb.*q4.*rk.*t3.*t5.*t13.*4.0+Mb.*q4.*rk.*t3.*t6.*t13.*2.0-Mb.*q4.*rk.*t4.*t5.*t13.*2.0+Mb.*q4.*rk.*t5.*t6.*t13.*2.0-Mb.*q1.*q2.*q3.*rk.*t5.*t8.*4.0-Mb.*q1.*q2.*q3.*rk.*t6.*t8.*4.0-Mb.*q1.*q2.*q3.*rk.*t4.*t13.*4.0-COM_X.*COM_Y.*Mb.*q1.*rk.*t3.*t4.*2.0-COM_X.*COM_Y.*Mb.*q1.*rk.*t3.*t5.*2.0+COM_X.*COM_Y.*Mb.*q1.*rk.*t4.*t6.*1.0e1+COM_X.*COM_Y.*Mb.*q1.*rk.*t5.*t6.*2.0-COM_X.*COM_Z.*Mb.*q2.*rk.*t4.*t5.*2.0-COM_Y.*COM_Z.*Mb.*q3.*rk.*t3.*t4.*6.0-COM_X.*COM_Z.*Mb.*q2.*rk.*t4.*t6.*6.0-COM_Y.*COM_Z.*Mb.*q3.*rk.*t3.*t5.*2.0-COM_X.*COM_Z.*Mb.*q2.*rk.*t5.*t6.*2.0-COM_Y.*COM_Z.*Mb.*q3.*rk.*t4.*t6.*2.0+COM_Y.*COM_Z.*Mb.*q3.*rk.*t5.*t6.*2.0-COM_X.*COM_Y.*Mb.*q2.*q3.*q4.*rk.*t3.*4.0-COM_X.*COM_Z.*Mb.*q1.*q3.*q4.*rk.*t3.*4.0-COM_X.*COM_Y.*Mb.*q2.*q3.*q4.*rk.*t4.*4.0-COM_X.*COM_Z.*Mb.*q1.*q3.*q4.*rk.*t4.*4.0-COM_Y.*COM_Z.*Mb.*q1.*q2.*q4.*rk.*t4.*8.0-COM_X.*COM_Z.*Mb.*q1.*q3.*q4.*rk.*t5.*4.0-COM_Y.*COM_Z.*Mb.*q1.*q2.*q4.*rk.*t5.*8.0-COM_X.*COM_Z.*Mb.*q1.*q3.*q4.*rk.*t6.*4.0).*2.0,Mb.*g.*rk.*t32.*(t66+t67+t68+t69+t70+t71+t72+t73+t74-t75-t76-t77-t78-t79-t80-t81+t82-t83-t84-t85-t86-t87+t88+t89+t90+t91-COM_X.*Jk.*q3-COM_X.*Mb.*q3.*t2-COM_X.*Mk.*q3.*t2+Mb.*q1.*rk.*t7.*t8.*2.0-Mb.*q1.*rk.*t7.*t11+Mb.*q1.*rk.*t7.*t13+Mb.*q1.*rk.*t8.*t12.*2.0+Mb.*q1.*rk.*t10.*t11.*3.0+Mb.*q1.*rk.*t9.*t13-Mb.*q1.*rk.*t10.*t13-Mb.*q1.*rk.*t12.*t13+COM_X.*COM_Z.*Mb.*q3.*rk.*t7+COM_X.*COM_Y.*Mb.*q4.*rk.*t9.*3.0-COM_X.*COM_Z.*Mb.*q3.*rk.*t9.*3.0+COM_Y.*COM_Z.*Mb.*q2.*rk.*t9+COM_Y.*COM_Z.*Mb.*q2.*rk.*t10+COM_X.*COM_Z.*Mb.*q3.*rk.*t12+COM_Y.*COM_Z.*Mb.*q2.*rk.*t12+Mb.*q1.*rk.*t3.*t5.*t8.*2.0+Mb.*q1.*rk.*t3.*t6.*t8.*2.0-Mb.*q1.*rk.*t4.*t5.*t8.*2.0+Mb.*q1.*rk.*t4.*t6.*t8.*6.0+Mb.*q1.*rk.*t5.*t6.*t8.*4.0+Mb.*q1.*rk.*t3.*t4.*t13.*4.0+Mb.*q1.*rk.*t3.*t5.*t13.*2.0+Mb.*q1.*rk.*t4.*t5.*t13.*4.0-Mb.*q1.*rk.*t4.*t6.*t13.*2.0-Mb.*q2.*q3.*q4.*rk.*t3.*t8.*4.0+Mb.*q2.*q3.*q4.*rk.*t4.*t8.*4.0+Mb.*q2.*q3.*q4.*rk.*t5.*t13.*4.0+COM_X.*COM_Y.*Mb.*q4.*rk.*t3.*t4.*1.0e1-COM_Y.*COM_Z.*Mb.*q2.*rk.*t3.*t4.*6.0+COM_X.*COM_Y.*Mb.*q4.*rk.*t3.*t5.*2.0-COM_X.*COM_Z.*Mb.*q3.*rk.*t3.*t5.*2.0-COM_Y.*COM_Z.*Mb.*q2.*rk.*t3.*t5.*2.0+COM_X.*COM_Y.*Mb.*q4.*rk.*t3.*t6.*2.0+COM_X.*COM_Y.*Mb.*q4.*rk.*t4.*t5.*6.0-COM_X.*COM_Z.*Mb.*q3.*rk.*t3.*t6.*2.0+COM_X.*COM_Z.*Mb.*q3.*rk.*t4.*t6.*6.0+COM_Y.*COM_Z.*Mb.*q2.*rk.*t4.*t6.*2.0+COM_X.*COM_Z.*Mb.*q3.*rk.*t5.*t6.*2.0+COM_Y.*COM_Z.*Mb.*q2.*rk.*t5.*t6.*6.0-COM_X.*COM_Y.*Mb.*q1.*q2.*q3.*rk.*t3.*4.0+COM_X.*COM_Y.*Mb.*q1.*q2.*q3.*rk.*t4.*4.0-COM_X.*COM_Z.*Mb.*q1.*q2.*q4.*rk.*t4.*8.0-COM_X.*COM_Z.*Mb.*q1.*q2.*q4.*rk.*t5.*8.0+COM_Y.*COM_Z.*Mb.*q1.*q3.*q4.*rk.*t3.*4.0+COM_Y.*COM_Z.*Mb.*q1.*q3.*q4.*rk.*t4.*4.0+COM_Y.*COM_Z.*Mb.*q1.*q3.*q4.*rk.*t5.*4.0+COM_Y.*COM_Z.*Mb.*q1.*q3.*q4.*rk.*t6.*4.0).*-2.0,Mb.*g.*rk.*t32.*(t66+t67+t68+t69+t70+t71+t72+t73+t74+t75+t76+t77+t78+t79+t80+t81+t82+t83+t84+t85+t86+t87+t88+t89+t90+t91-COM_X.*Jk.*q3-COM_X.*Mb.*q3.*t2-COM_X.*Mk.*q3.*t2-Mb.*q1.*rk.*t7.*t8+Mb.*q1.*rk.*t8.*t9+Mb.*q1.*rk.*t7.*t11.*3.0+Mb.*q1.*rk.*t8.*t10-Mb.*q1.*rk.*t8.*t12-Mb.*q1.*rk.*t10.*t11+Mb.*q1.*rk.*t10.*t13.*2.0+Mb.*q1.*rk.*t12.*t13.*2.0-COM_X.*COM_Z.*Mb.*q3.*rk.*t7-COM_X.*COM_Y.*Mb.*q4.*rk.*t9.*3.0-COM_X.*COM_Z.*Mb.*q3.*rk.*t9+COM_Y.*COM_Z.*Mb.*q2.*rk.*t9.*3.0-COM_Y.*COM_Z.*Mb.*q2.*rk.*t10-COM_X.*COM_Z.*Mb.*q3.*rk.*t12-COM_Y.*COM_Z.*Mb.*q2.*rk.*t12+Mb.*q1.*rk.*t3.*t4.*t8.*2.0+Mb.*q1.*rk.*t3.*t5.*t8.*4.0+Mb.*q1.*rk.*t4.*t5.*t8.*4.0-Mb.*q1.*rk.*t5.*t6.*t8.*2.0+Mb.*q1.*rk.*t3.*t4.*t13.*2.0+Mb.*q1.*rk.*t3.*t6.*t13.*2.0-Mb.*q1.*rk.*t4.*t5.*t13.*2.0+Mb.*q1.*rk.*t4.*t6.*t13.*4.0+Mb.*q1.*rk.*t5.*t6.*t13.*6.0-Mb.*q2.*q3.*q4.*rk.*t3.*t8.*4.0-Mb.*q2.*q3.*q4.*rk.*t4.*t8.*4.0-Mb.*q2.*q3.*q4.*rk.*t5.*t13.*4.0-COM_X.*COM_Y.*Mb.*q4.*rk.*t3.*t4.*2.0+COM_Y.*COM_Z.*Mb.*q2.*rk.*t3.*t4.*2.0-COM_X.*COM_Y.*Mb.*q4.*rk.*t3.*t5.*1.0e1+COM_X.*COM_Z.*Mb.*q3.*rk.*t3.*t5.*6.0-COM_Y.*COM_Z.*Mb.*q2.*rk.*t3.*t5.*2.0-COM_X.*COM_Y.*Mb.*q4.*rk.*t3.*t6.*2.0-COM_X.*COM_Y.*Mb.*q4.*rk.*t4.*t5.*6.0-COM_X.*COM_Z.*Mb.*q3.*rk.*t3.*t6.*2.0-COM_X.*COM_Z.*Mb.*q3.*rk.*t4.*t6.*6.0-COM_Y.*COM_Z.*Mb.*q2.*rk.*t4.*t6.*2.0-COM_X.*COM_Z.*Mb.*q3.*rk.*t5.*t6.*2.0-COM_Y.*COM_Z.*Mb.*q2.*rk.*t5.*t6.*6.0-COM_X.*COM_Y.*Mb.*q1.*q2.*q3.*rk.*t3.*4.0-COM_X.*COM_Y.*Mb.*q1.*q2.*q3.*rk.*t4.*4.0+COM_X.*COM_Z.*Mb.*q1.*q2.*q4.*rk.*t3.*4.0+COM_X.*COM_Z.*Mb.*q1.*q2.*q4.*rk.*t4.*4.0+COM_X.*COM_Z.*Mb.*q1.*q2.*q4.*rk.*t5.*4.0+COM_X.*COM_Z.*Mb.*q1.*q2.*q4.*rk.*t6.*4.0-COM_Y.*COM_Z.*Mb.*q1.*q3.*q4.*rk.*t4.*8.0-COM_Y.*COM_Z.*Mb.*q1.*q3.*q4.*rk.*t5.*8.0).*2.0,Mb.*g.*rk.*t32.*(t92+t93+t94+t95+t96+t97+t98+t99+t100+t101+t102+t103-t104-t105-t106-t107-t108-t109+t110-t111-t112-t113+t114-t115-t116-t117+t118+t119+t120+Mb.*q4.*rk.*t8.*t9.*2.0+Mb.*q4.*rk.*t7.*t11.*3.0+Mb.*q4.*rk.*t8.*t10.*2.0-Mb.*q4.*rk.*t7.*t13-Mb.*q4.*rk.*t10.*t11-Mb.*q4.*rk.*t9.*t13+Mb.*q4.*rk.*t10.*t13+Mb.*q4.*rk.*t12.*t13+COM_X.*COM_Y.*Mb.*q1.*rk.*t7+COM_X.*COM_Y.*Mb.*q1.*rk.*t9+COM_X.*COM_Z.*Mb.*q2.*rk.*t7.*3.0+COM_X.*COM_Y.*Mb.*q1.*rk.*t10+COM_Y.*COM_Z.*Mb.*q3.*rk.*t7+COM_Y.*COM_Z.*Mb.*q3.*rk.*t9+COM_X.*COM_Z.*Mb.*q2.*rk.*t12.*3.0+COM_Y.*COM_Z.*Mb.*q3.*rk.*t12+Mb.*q4.*rk.*t3.*t4.*t8.*4.0+Mb.*q4.*rk.*t3.*t5.*t8.*6.0+Mb.*q4.*rk.*t3.*t6.*t8.*2.0-Mb.*q4.*rk.*t4.*t5.*t8.*2.0+Mb.*q4.*rk.*t4.*t6.*t8.*2.0-Mb.*q4.*rk.*t3.*t5.*t13.*2.0+Mb.*q4.*rk.*t4.*t5.*t13.*4.0+Mb.*q4.*rk.*t4.*t6.*t13.*2.0+Mb.*q4.*rk.*t5.*t6.*t13.*4.0+Mb.*q1.*q2.*q3.*rk.*t5.*t8.*4.0-Mb.*q1.*q2.*q3.*rk.*t6.*t8.*4.0+Mb.*q1.*q2.*q3.*rk.*t4.*t13.*4.0+COM_X.*COM_Y.*Mb.*q1.*rk.*t3.*t4.*2.0+COM_X.*COM_Y.*Mb.*q1.*rk.*t3.*t5.*2.0-COM_X.*COM_Y.*Mb.*q1.*rk.*t4.*t6.*2.0-COM_X.*COM_Y.*Mb.*q1.*rk.*t5.*t6.*1.0e1+COM_X.*COM_Z.*Mb.*q2.*rk.*t4.*t5.*2.0+COM_Y.*COM_Z.*Mb.*q3.*rk.*t3.*t4.*6.0+COM_X.*COM_Z.*Mb.*q2.*rk.*t4.*t6.*2.0+COM_Y.*COM_Z.*Mb.*q3.*rk.*t3.*t5.*2.0-COM_X.*COM_Z.*Mb.*q2.*rk.*t5.*t6.*2.0-COM_Y.*COM_Z.*Mb.*q3.*rk.*t4.*t6.*2.0-COM_Y.*COM_Z.*Mb.*q3.*rk.*t5.*t6.*6.0-COM_X.*COM_Y.*Mb.*q2.*q3.*q4.*rk.*t3.*4.0+COM_Y.*COM_Z.*Mb.*q1.*q2.*q4.*rk.*t3.*4.0+COM_X.*COM_Y.*Mb.*q2.*q3.*q4.*rk.*t4.*4.0+COM_X.*COM_Z.*Mb.*q1.*q3.*q4.*rk.*t4.*8.0+COM_Y.*COM_Z.*Mb.*q1.*q2.*q4.*rk.*t4.*4.0+COM_X.*COM_Z.*Mb.*q1.*q3.*q4.*rk.*t5.*8.0+COM_Y.*COM_Z.*Mb.*q1.*q2.*q4.*rk.*t5.*4.0+COM_Y.*COM_Z.*Mb.*q1.*q2.*q4.*rk.*t6.*4.0).*2.0,Mb.*g.*rk.*t32.*(t40+t41+t42+t43+t44+t45+t46+t47+t48+t49+t50+t51+t52+t53+t54+t55+t56+t57+t58+t59+t60+t61+t62+t63+t64+t65-COM_Z.*Jk.*q2-COM_Z.*Mb.*q2.*t2-COM_Z.*Mk.*q2.*t2-Mb.*q2.*rk.*t7.*t8+Mb.*q2.*rk.*t8.*t9+Mb.*q2.*rk.*t8.*t10+Mb.*q2.*rk.*t7.*t13.*2.0-Mb.*q2.*rk.*t8.*t12-Mb.*q2.*rk.*t9.*t11+Mb.*q2.*rk.*t9.*t13.*2.0+Mb.*q2.*rk.*t11.*t12.*3.0-COM_X.*COM_Y.*Mb.*q3.*rk.*t7-COM_X.*COM_Y.*Mb.*q3.*rk.*t9-COM_X.*COM_Z.*Mb.*q4.*rk.*t7-COM_Y.*COM_Z.*Mb.*q1.*rk.*t10.*3.0-COM_X.*COM_Y.*Mb.*q3.*rk.*t12-COM_X.*COM_Z.*Mb.*q4.*rk.*t10-COM_Y.*COM_Z.*Mb.*q1.*rk.*t12.*3.0-COM_X.*COM_Z.*Mb.*q4.*rk.*t12+Mb.*q2.*rk.*t3.*t4.*t8.*2.0+Mb.*q2.*rk.*t3.*t6.*t8.*4.0+Mb.*q2.*rk.*t4.*t6.*t8.*4.0-Mb.*q2.*rk.*t5.*t6.*t8.*2.0+Mb.*q2.*rk.*t3.*t4.*t13.*2.0+Mb.*q2.*rk.*t3.*t5.*t13.*4.0-Mb.*q2.*rk.*t3.*t6.*t13.*2.0+Mb.*q2.*rk.*t4.*t5.*t13.*2.0+Mb.*q2.*rk.*t5.*t6.*t13.*6.0-Mb.*q1.*q3.*q4.*rk.*t3.*t8.*4.0-Mb.*q1.*q3.*q4.*rk.*t4.*t8.*4.0-Mb.*q1.*q3.*q4.*rk.*t6.*t13.*4.0+COM_X.*COM_Y.*Mb.*q3.*rk.*t3.*t4.*2.0-COM_Y.*COM_Z.*Mb.*q1.*rk.*t3.*t4.*2.0-COM_X.*COM_Y.*Mb.*q3.*rk.*t3.*t5.*2.0-COM_Y.*COM_Z.*Mb.*q1.*rk.*t3.*t6.*2.0-COM_Y.*COM_Z.*Mb.*q1.*rk.*t4.*t5.*2.0+COM_X.*COM_Y.*Mb.*q3.*rk.*t4.*t6.*1.0e1-COM_X.*COM_Z.*Mb.*q4.*rk.*t3.*t5.*6.0-COM_X.*COM_Y.*Mb.*q3.*rk.*t5.*t6.*2.0-COM_X.*COM_Z.*Mb.*q4.*rk.*t4.*t5.*2.0+COM_X.*COM_Z.*Mb.*q4.*rk.*t4.*t6.*6.0-COM_X.*COM_Z.*Mb.*q4.*rk.*t5.*t6.*2.0+COM_X.*COM_Z.*Mb.*q1.*q2.*q3.*rk.*t3.*4.0+COM_X.*COM_Z.*Mb.*q1.*q2.*q3.*rk.*t4.*4.0-COM_X.*COM_Y.*Mb.*q1.*q2.*q4.*rk.*t5.*4.0+COM_X.*COM_Z.*Mb.*q1.*q2.*q3.*rk.*t5.*4.0-COM_X.*COM_Y.*Mb.*q1.*q2.*q4.*rk.*t6.*4.0+COM_X.*COM_Z.*Mb.*q1.*q2.*q3.*rk.*t6.*4.0+COM_Y.*COM_Z.*Mb.*q2.*q3.*q4.*rk.*t3.*8.0+COM_Y.*COM_Z.*Mb.*q2.*q3.*q4.*rk.*t6.*8.0).*-2.0,Mb.*g.*rk.*t32.*(t121+t122+t123+t124+t125+t126-t127-t128-t129-t130-t131-t132-t133-t134-t135+t136-t137-t138-t139-t140-t141+t142-t143-t144+t145+t146+t147-COM_X.*Jk.*q1-COM_Z.*Jk.*q3-COM_X.*Mb.*q1.*t2-COM_Z.*Mb.*q3.*t2-COM_X.*Mk.*q1.*t2-COM_Z.*Mk.*q3.*t2+Mb.*q3.*rk.*t8.*t9.*2.0+Mb.*q3.*rk.*t8.*t10.*2.0+Mb.*q3.*rk.*t7.*t13-Mb.*q3.*rk.*t9.*t11+Mb.*q3.*rk.*t9.*t13-Mb.*q3.*rk.*t10.*t13+Mb.*q3.*rk.*t11.*t12.*3.0-Mb.*q3.*rk.*t12.*t13+COM_X.*COM_Y.*Mb.*q2.*rk.*t7.*3.0+COM_X.*COM_Z.*Mb.*q1.*rk.*t7.*3.0-COM_Y.*COM_Z.*Mb.*q4.*rk.*t7+COM_X.*COM_Z.*Mb.*q1.*rk.*t12.*3.0+COM_Y.*COM_Z.*Mb.*q4.*rk.*t9.*3.0+Mb.*q3.*rk.*t3.*t4.*t8.*4.0+Mb.*q3.*rk.*t3.*t5.*t8.*2.0-Mb.*q3.*rk.*t3.*t6.*t8.*2.0+Mb.*q3.*rk.*t4.*t5.*t8.*2.0+Mb.*q3.*rk.*t4.*t6.*t8.*6.0+Mb.*q3.*rk.*t3.*t5.*t13.*2.0+Mb.*q3.*rk.*t3.*t6.*t13.*4.0-Mb.*q3.*rk.*t4.*t6.*t13.*2.0+Mb.*q3.*rk.*t5.*t6.*t13.*4.0-Mb.*q1.*q2.*q4.*rk.*t5.*t8.*4.0+Mb.*q1.*q2.*q4.*rk.*t6.*t8.*4.0+Mb.*q1.*q2.*q4.*rk.*t3.*t13.*4.0+COM_X.*COM_Y.*Mb.*q2.*rk.*t3.*t5.*2.0+COM_X.*COM_Z.*Mb.*q1.*rk.*t3.*t5.*2.0+COM_X.*COM_Y.*Mb.*q2.*rk.*t3.*t6.*6.0+COM_X.*COM_Y.*Mb.*q2.*rk.*t4.*t5.*2.0+COM_X.*COM_Z.*Mb.*q1.*rk.*t3.*t6.*2.0+COM_X.*COM_Y.*Mb.*q2.*rk.*t5.*t6.*1.0e1-COM_X.*COM_Z.*Mb.*q1.*rk.*t5.*t6.*2.0+COM_Y.*COM_Z.*Mb.*q4.*rk.*t3.*t6.*2.0-COM_Y.*COM_Z.*Mb.*q4.*rk.*t4.*t5.*2.0+COM_Y.*COM_Z.*Mb.*q4.*rk.*t5.*t6.*6.0-COM_Y.*COM_Z.*Mb.*q1.*q2.*q3.*rk.*t3.*4.0-COM_Y.*COM_Z.*Mb.*q1.*q2.*q3.*rk.*t4.*4.0-COM_X.*COM_Y.*Mb.*q1.*q3.*q4.*rk.*t5.*4.0+COM_X.*COM_Z.*Mb.*q2.*q3.*q4.*rk.*t3.*8.0-COM_Y.*COM_Z.*Mb.*q1.*q2.*q3.*rk.*t5.*4.0+COM_X.*COM_Y.*Mb.*q1.*q3.*q4.*rk.*t6.*4.0-COM_Y.*COM_Z.*Mb.*q1.*q2.*q3.*rk.*t6.*4.0+COM_X.*COM_Z.*Mb.*q2.*q3.*q4.*rk.*t6.*8.0).*-2.0],[2,4]); 
+  f0 = (((((((((((((((((((((((((((((t121 + t122) + t123) + t124) + t125) + t126)
+    + t127) + t128) + t129) + t130) + t131) + t132) + t133) + t134) + t135) +
+                      t136) + t137) + t138) + t139) + t140) + t141) + t142) +
+               t143) + t144) + t145) + t146) + t147) - COM_X * Jk * q1) - COM_Z *
+         Jk * q3) - COM_X * Mb * q1 * t2) - COM_Z * Mb * q3 * t2;
+  f1 = COM_X * Mk * q1 * t2;
+  f2 = COM_Z * Mk * q3;
+  f3 = Mb * q3 * rk * t7;
+  f4 = Mb * q3 * rk;
+  f5 = Mb * q3 * rk;
+  f6 = Mb * q3;
+  f7 = Mb * q3 * rk;
+  f8 = Mb * q3;
+  f9 = COM_X * COM_Y;
+  x[0] = Mb * g * rk * t32 * (((((((((((((((((((((((((((((((((((((((((((((f0 -
+    f1) - f2 * t2) + f3 * t8) - f4 * t8 * t9) - f5 * t8 * t10) + f6 * rk * t8 *
+    t12) + f7 * t9 * t11 * 3.0F) + f8 * rk * t10 * t13 * 2.0F) - Mb * q3 * rk *
+    t11 * t12) + Mb * q3 * rk * t12 * t13 * 2.0F) - f9 * Mb * q2 * rk * t7 *
+    3.0F) + COM_X * COM_Z * Mb * q1 * rk * t7) - COM_Y * COM_Z * Mb * q4 * rk *
+    t7 * 3.0F) - COM_X * COM_Z * Mb * q1 * rk * t12 * 3.0F) - COM_Y * COM_Z * Mb
+    * q4 * rk * t9 * 3.0F) - Mb * q3 * rk * t3 * t4 * t8 * 2.0F) + Mb * q3 * rk *
+    t3 * t5 * t8 * 4.0F) + Mb * q3 * rk * t3 * t6 * t8 * 4.0F) + Mb * q3 * rk *
+    t5 * t6 * t8 * 2.0F) + Mb * q3 * rk * t3 * t4 * t13 * 6.0F) - Mb * q3 * rk *
+    t3 * t6 * t13 * 2.0F) + Mb * q3 * rk * t4 * t5 * t13 * 2.0F) + Mb * q3 * rk *
+    t4 * t6 * t13 * 4.0F) + Mb * q3 * rk * t5 * t6 * t13 * 2.0F) - Mb * q1 * q2 *
+    q4 * rk * t5 * t8 * 4.0F) - Mb * q1 * q2 * q4 * rk * t6 * t8 * 4.0F) - Mb *
+    q1 * q2 * q4 * rk * t3 * t13 * 4.0F) - COM_X * COM_Y * Mb * q2 * rk * t3 *
+    t5 * 10.0F) - COM_X * COM_Z * Mb * q1 * rk * t3 * t5 * 6.0F) - COM_X * COM_Y
+    * Mb * q2 * rk * t3 * t6 * 6.0F) - COM_X * COM_Y * Mb * q2 * rk * t4 * t5 *
+    2.0F) - COM_X * COM_Z * Mb * q1 * rk * t3 * t6 * 2.0F) - COM_X * COM_Y * Mb *
+    q2 * rk * t5 * t6 * 2.0F) - COM_X * COM_Z * Mb * q1 * rk * t5 * t6 * 2.0F) -
+    COM_Y * COM_Z * Mb * q4 * rk * t3 * t6 * 2.0F) - COM_Y * COM_Z * Mb * q4 *
+    rk * t4 * t5 * 2.0F) - COM_Y * COM_Z * Mb * q4 * rk * t5 * t6 * 2.0F) +
+    COM_Y * COM_Z * Mb * q1 * q2 * q3 * rk * t3 * 8.0F) - COM_X * COM_Y * Mb *
+    q1 * q3 * q4 * rk * t5 * 4.0F) - COM_X * COM_Z * Mb * q2 * q3 * q4 * rk * t3
+    * 4.0F) - COM_X * COM_Y * Mb * q1 * q3 * q4 * rk * t6 * 4.0F) - COM_X *
+    COM_Z * Mb * q2 * q3 * q4 * rk * t4 * 4.0F) + COM_Y * COM_Z * Mb * q1 * q2 *
+    q3 * rk * t6 * 8.0F) - COM_X * COM_Z * Mb * q2 * q3 * q4 * rk * t5 * 4.0F) -
+    COM_X * COM_Z * Mb * q2 * q3 * q4 * rk * t6 * 4.0F) * -2.0F;
+  f0 = (((((((((((((((((((((((((((((t40 + t41) + t42) + t43) + t44) + t45) + t46)
+    + t47) + t48) - t49) - t50) - t51) - t52) - t53) - t54) + t55) - t56) - t57)
+                   - t58) + t59) - t60) - t61) + t62) + t63) - t64) + t65) -
+           COM_Z * Jk * q2) - COM_Z * Mb * q2 * t2) - COM_Z * Mk * q2 * t2) + Mb
+        * q2 * rk * t7 * t8 * 2.0F) - Mb * q2 * rk * t7 * t13;
+  f1 = Mb * q2 * rk * t8 * t12 * 2.0F;
+  f2 = Mb * q2 * rk * t9 * t11;
+  f3 = Mb * q2 * rk * t9;
+  f4 = Mb * q2 * rk;
+  f5 = Mb * q2 * rk;
+  f6 = Mb * q2;
+  f7 = COM_X * COM_Y * Mb;
+  f8 = COM_X * COM_Y;
+  f9 = COM_X * COM_Z;
+  x[1] = Mb * g * rk * t32 * (((((((((((((((((((((((((((((((((((((((((((((f0 +
+    f1) + f2 * 3.0F) - f3 * t13) + f4 * t10 * t13) - f5 * t11 * t12) + f6 * rk *
+    t12 * t13) + f7 * q3 * rk * t7) + f8 * Mb * q3 * rk * t9) + f9 * Mb * q4 *
+    rk * t7) - COM_Y * COM_Z * Mb * q1 * rk * t10) + COM_X * COM_Y * Mb * q3 *
+    rk * t12) - COM_X * COM_Z * Mb * q4 * rk * t10 * 3.0F) + COM_Y * COM_Z * Mb *
+    q1 * rk * t12 * 3.0F) + COM_X * COM_Z * Mb * q4 * rk * t12) + Mb * q2 * rk *
+    t3 * t5 * t8 * 6.0F) - Mb * q2 * rk * t3 * t6 * t8 * 2.0F) + Mb * q2 * rk *
+    t4 * t5 * t8 * 2.0F) + Mb * q2 * rk * t4 * t6 * t8 * 2.0F) + Mb * q2 * rk *
+    t5 * t6 * t8 * 4.0F) + Mb * q2 * rk * t3 * t4 * t13 * 4.0F) - Mb * q2 * rk *
+    t3 * t5 * t13 * 2.0F) + Mb * q2 * rk * t3 * t6 * t13 * 4.0F) + Mb * q2 * rk *
+    t4 * t6 * t13 * 2.0F) + Mb * q1 * q3 * q4 * rk * t3 * t8 * 4.0F) - Mb * q1 *
+    q3 * q4 * rk * t4 * t8 * 4.0F) + Mb * q1 * q3 * q4 * rk * t6 * t13 * 4.0F) -
+    COM_X * COM_Y * Mb * q3 * rk * t3 * t4 * 10.0F) + COM_Y * COM_Z * Mb * q1 *
+    rk * t3 * t4 * 6.0F) + COM_X * COM_Y * Mb * q3 * rk * t3 * t5 * 2.0F) +
+    COM_Y * COM_Z * Mb * q1 * rk * t3 * t6 * 2.0F) - COM_Y * COM_Z * Mb * q1 *
+    rk * t4 * t5 * 2.0F) - COM_X * COM_Y * Mb * q3 * rk * t4 * t6 * 2.0F) +
+    COM_X * COM_Z * Mb * q4 * rk * t3 * t5 * 6.0F) + COM_X * COM_Y * Mb * q3 *
+    rk * t5 * t6 * 2.0F) - COM_X * COM_Z * Mb * q4 * rk * t4 * t5 * 2.0F) -
+    COM_X * COM_Z * Mb * q4 * rk * t4 * t6 * 2.0F) + COM_X * COM_Z * Mb * q4 *
+    rk * t5 * t6 * 2.0F) - COM_X * COM_Z * Mb * q1 * q2 * q3 * rk * t3 * 8.0F) -
+    COM_X * COM_Y * Mb * q1 * q2 * q4 * rk * t5 * 4.0F) + COM_X * COM_Y * Mb *
+    q1 * q2 * q4 * rk * t6 * 4.0F) - COM_X * COM_Z * Mb * q1 * q2 * q3 * rk * t6
+    * 8.0F) - COM_Y * COM_Z * Mb * q2 * q3 * q4 * rk * t3 * 4.0F) - COM_Y *
+    COM_Z * Mb * q2 * q3 * q4 * rk * t4 * 4.0F) - COM_Y * COM_Z * Mb * q2 * q3 *
+    q4 * rk * t5 * 4.0F) - COM_Y * COM_Z * Mb * q2 * q3 * q4 * rk * t6 * 4.0F) *
+    2.0F;
+  f0 = (((((((((((((((((((((((((((((t92 + t93) + t94) + t95) + t96) + t97) + t98)
+    + t99) + t100) + t101) + t102) + t103) + t104) + t105) + t106) + t107) +
+                     t108) + t109) + t110) + t111) + t112) + t113) + t114) +
+              t115) + t116) + t117) + t118) + t119) + t120) + Mb * q4 * rk * t7 *
+        t8) - Mb * q4 * rk * t8 * t9;
+  f1 = Mb * q4 * rk * t7 * t11;
+  f2 = Mb * q4 * rk * t8;
+  f3 = Mb * q4 * rk * t7 * t13;
+  f4 = Mb * q4 * rk;
+  f5 = Mb * q4 * rk * t10;
+  f6 = Mb * q4 * rk;
+  f7 = COM_X * COM_Y * Mb;
+  f8 = COM_X * COM_Y;
+  f9 = COM_X * COM_Z * Mb;
+  x[2] = Mb * g * rk * t32 * (((((((((((((((((((((((((((((((((((((((((((((f0 -
+    f1) - f2 * t10) + f3 * 2.0F) + f4 * t8 * t12) + f5 * t11 * 3.0F) + f6 * t9 *
+    t13 * 2.0F) - f7 * q1 * rk * t7) - f8 * Mb * q1 * rk * t9) - f9 * q2 * rk *
+    t7 * 3.0F) - COM_X * COM_Y * Mb * q1 * rk * t10) - COM_Y * COM_Z * Mb * q3 *
+    rk * t7) - COM_Y * COM_Z * Mb * q3 * rk * t9) + COM_X * COM_Z * Mb * q2 * rk
+    * t12) + COM_Y * COM_Z * Mb * q3 * rk * t12 * 3.0F) - Mb * q4 * rk * t3 * t4
+    * t8 * 2.0F) + Mb * q4 * rk * t4 * t5 * t8 * 4.0F) + Mb * q4 * rk * t4 * t6 *
+    t8 * 4.0F) + Mb * q4 * rk * t5 * t6 * t8 * 2.0F) + Mb * q4 * rk * t3 * t4 *
+    t13 * 6.0F) + Mb * q4 * rk * t3 * t5 * t13 * 4.0F) + Mb * q4 * rk * t3 * t6 *
+    t13 * 2.0F) - Mb * q4 * rk * t4 * t5 * t13 * 2.0F) + Mb * q4 * rk * t5 * t6 *
+    t13 * 2.0F) - Mb * q1 * q2 * q3 * rk * t5 * t8 * 4.0F) - Mb * q1 * q2 * q3 *
+    rk * t6 * t8 * 4.0F) - Mb * q1 * q2 * q3 * rk * t4 * t13 * 4.0F) - COM_X *
+    COM_Y * Mb * q1 * rk * t3 * t4 * 2.0F) - COM_X * COM_Y * Mb * q1 * rk * t3 *
+    t5 * 2.0F) + COM_X * COM_Y * Mb * q1 * rk * t4 * t6 * 10.0F) + COM_X * COM_Y
+    * Mb * q1 * rk * t5 * t6 * 2.0F) - COM_X * COM_Z * Mb * q2 * rk * t4 * t5 *
+    2.0F) - COM_Y * COM_Z * Mb * q3 * rk * t3 * t4 * 6.0F) - COM_X * COM_Z * Mb *
+    q2 * rk * t4 * t6 * 6.0F) - COM_Y * COM_Z * Mb * q3 * rk * t3 * t5 * 2.0F) -
+    COM_X * COM_Z * Mb * q2 * rk * t5 * t6 * 2.0F) - COM_Y * COM_Z * Mb * q3 *
+    rk * t4 * t6 * 2.0F) + COM_Y * COM_Z * Mb * q3 * rk * t5 * t6 * 2.0F) -
+    COM_X * COM_Y * Mb * q2 * q3 * q4 * rk * t3 * 4.0F) - COM_X * COM_Z * Mb *
+    q1 * q3 * q4 * rk * t3 * 4.0F) - COM_X * COM_Y * Mb * q2 * q3 * q4 * rk * t4
+    * 4.0F) - COM_X * COM_Z * Mb * q1 * q3 * q4 * rk * t4 * 4.0F) - COM_Y *
+    COM_Z * Mb * q1 * q2 * q4 * rk * t4 * 8.0F) - COM_X * COM_Z * Mb * q1 * q3 *
+    q4 * rk * t5 * 4.0F) - COM_Y * COM_Z * Mb * q1 * q2 * q4 * rk * t5 * 8.0F) -
+    COM_X * COM_Z * Mb * q1 * q3 * q4 * rk * t6 * 4.0F) * 2.0F;
+  f0 = (((((((((((((((((((((((((((((t31 + t67) + t68) + t69) + t70) + t71) + t72)
+    + t73) + t74) - t75) - t76) - t77) - t78) - t79) - t80) - t81) + t82) - t83)
+                   - t84) - t85) - t86) - t87) + t88) + t89) + t90) + t91) -
+           COM_X * Jk * q3) - COM_X * Mb * q3 * t2) - COM_X * Mk * q3 * t2) + Mb
+        * q1 * rk * t7 * t8 * 2.0F) - Mb * q1 * rk * t7 * t11;
+  f1 = Mb * q1 * rk * t7 * t13;
+  f2 = Mb * q1 * rk * t8 * t12;
+  f3 = Mb * q1 * rk * t10 * t11;
+  f4 = Mb * q1 * rk;
+  f5 = Mb * q1 * rk;
+  f6 = Mb * q1;
+  f7 = COM_X * COM_Z * Mb;
+  f8 = COM_X * COM_Y * Mb;
+  f9 = COM_X * COM_Z * Mb;
+  x[3] = Mb * g * rk * t32 * (((((((((((((((((((((((((((((((((((((((((((((f0 +
+    f1) + f2 * 2.0F) + f3 * 3.0F) + f4 * t9 * t13) - f5 * t10 * t13) - f6 * rk *
+    t12 * t13) + f7 * q3 * rk * t7) + f8 * q4 * rk * t9 * 3.0F) - f9 * q3 * rk *
+    t9 * 3.0F) + COM_Y * COM_Z * Mb * q2 * rk * t9) + COM_Y * COM_Z * Mb * q2 *
+    rk * t10) + COM_X * COM_Z * Mb * q3 * rk * t12) + COM_Y * COM_Z * Mb * q2 *
+    rk * t12) + Mb * q1 * rk * t3 * t5 * t8 * 2.0F) + Mb * q1 * rk * t3 * t6 *
+    t8 * 2.0F) - Mb * q1 * rk * t4 * t5 * t8 * 2.0F) + Mb * q1 * rk * t4 * t6 *
+    t8 * 6.0F) + Mb * q1 * rk * t5 * t6 * t8 * 4.0F) + Mb * q1 * rk * t3 * t4 *
+    t13 * 4.0F) + Mb * q1 * rk * t3 * t5 * t13 * 2.0F) + Mb * q1 * rk * t4 * t5 *
+    t13 * 4.0F) - Mb * q1 * rk * t4 * t6 * t13 * 2.0F) - Mb * q2 * q3 * q4 * rk *
+    t3 * t8 * 4.0F) + Mb * q2 * q3 * q4 * rk * t4 * t8 * 4.0F) + Mb * q2 * q3 *
+    q4 * rk * t5 * t13 * 4.0F) + COM_X * COM_Y * Mb * q4 * rk * t3 * t4 * 10.0F)
+    - COM_Y * COM_Z * Mb * q2 * rk * t3 * t4 * 6.0F) + COM_X * COM_Y * Mb * q4 *
+    rk * t3 * t5 * 2.0F) - COM_X * COM_Z * Mb * q3 * rk * t3 * t5 * 2.0F) -
+    COM_Y * COM_Z * Mb * q2 * rk * t3 * t5 * 2.0F) + COM_X * COM_Y * Mb * q4 *
+    rk * t3 * t6 * 2.0F) + COM_X * COM_Y * Mb * q4 * rk * t4 * t5 * 6.0F) -
+    COM_X * COM_Z * Mb * q3 * rk * t3 * t6 * 2.0F) + COM_X * COM_Z * Mb * q3 *
+    rk * t4 * t6 * 6.0F) + COM_Y * COM_Z * Mb * q2 * rk * t4 * t6 * 2.0F) +
+    COM_X * COM_Z * Mb * q3 * rk * t5 * t6 * 2.0F) + COM_Y * COM_Z * Mb * q2 *
+    rk * t5 * t6 * 6.0F) - COM_X * COM_Y * Mb * q1 * q2 * q3 * rk * t3 * 4.0F) +
+    COM_X * COM_Y * Mb * q1 * q2 * q3 * rk * t4 * 4.0F) - COM_X * COM_Z * Mb *
+    q1 * q2 * q4 * rk * t4 * 8.0F) - COM_X * COM_Z * Mb * q1 * q2 * q4 * rk * t5
+    * 8.0F) + COM_Y * COM_Z * Mb * q1 * q3 * q4 * rk * t3 * 4.0F) + COM_Y *
+    COM_Z * Mb * q1 * q3 * q4 * rk * t4 * 4.0F) + COM_Y * COM_Z * Mb * q1 * q3 *
+    q4 * rk * t5 * 4.0F) + COM_Y * COM_Z * Mb * q1 * q3 * q4 * rk * t6 * 4.0F) *
+    -2.0F;
+  f0 = (((((((((((((((((((((((((((((t31 + t67) + t68) + t69) + t70) + t71) + t72)
+    + t73) + t74) + t75) + t76) + t77) + t78) + t79) + t80) + t81) + t82) + t83)
+                   + t84) + t85) + t86) + t87) + t88) + t89) + t90) + t91) -
+           COM_X * Jk * q3) - COM_X * Mb * q3 * t2) - COM_X * Mk * q3 * t2) - Mb
+        * q1 * rk * t7 * t8) + Mb * q1 * rk * t8 * t9;
+  f1 = Mb * q1 * rk * t7 * t11 * 3.0F;
+  f2 = Mb * q1 * rk * t8;
+  f3 = Mb * q1 * rk * t8;
+  f4 = Mb * q1 * rk;
+  f5 = Mb * q1 * rk * t10;
+  f6 = Mb * q1 * rk;
+  f7 = COM_X * COM_Z * Mb;
+  f8 = COM_X * COM_Y * Mb;
+  f9 = COM_X * COM_Z;
+  t31 = COM_Y * COM_Z;
+  x[4] = Mb * g * rk * t32 * (((((((((((((((((((((((((((((((((((((((((((((f0 +
+    f1) + f2 * t10) - f3 * t12) - f4 * t10 * t11) + f5 * t13 * 2.0F) + f6 * t12 *
+    t13 * 2.0F) - f7 * q3 * rk * t7) - f8 * q4 * rk * t9 * 3.0F) - f9 * Mb * q3 *
+    rk * t9) + t31 * Mb * q2 * rk * t9 * 3.0F) - COM_Y * COM_Z * Mb * q2 * rk *
+    t10) - COM_X * COM_Z * Mb * q3 * rk * t12) - COM_Y * COM_Z * Mb * q2 * rk *
+    t12) + Mb * q1 * rk * t3 * t4 * t8 * 2.0F) + Mb * q1 * rk * t3 * t5 * t8 *
+    4.0F) + Mb * q1 * rk * t4 * t5 * t8 * 4.0F) - Mb * q1 * rk * t5 * t6 * t8 *
+    2.0F) + Mb * q1 * rk * t3 * t4 * t13 * 2.0F) + Mb * q1 * rk * t3 * t6 * t13 *
+    2.0F) - Mb * q1 * rk * t4 * t5 * t13 * 2.0F) + Mb * q1 * rk * t4 * t6 * t13 *
+    4.0F) + Mb * q1 * rk * t5 * t6 * t13 * 6.0F) - Mb * q2 * q3 * q4 * rk * t3 *
+    t8 * 4.0F) - Mb * q2 * q3 * q4 * rk * t4 * t8 * 4.0F) - Mb * q2 * q3 * q4 *
+    rk * t5 * t13 * 4.0F) - COM_X * COM_Y * Mb * q4 * rk * t3 * t4 * 2.0F) +
+    COM_Y * COM_Z * Mb * q2 * rk * t3 * t4 * 2.0F) - COM_X * COM_Y * Mb * q4 *
+    rk * t3 * t5 * 10.0F) + COM_X * COM_Z * Mb * q3 * rk * t3 * t5 * 6.0F) -
+    COM_Y * COM_Z * Mb * q2 * rk * t3 * t5 * 2.0F) - COM_X * COM_Y * Mb * q4 *
+    rk * t3 * t6 * 2.0F) - COM_X * COM_Y * Mb * q4 * rk * t4 * t5 * 6.0F) -
+    COM_X * COM_Z * Mb * q3 * rk * t3 * t6 * 2.0F) - COM_X * COM_Z * Mb * q3 *
+    rk * t4 * t6 * 6.0F) - COM_Y * COM_Z * Mb * q2 * rk * t4 * t6 * 2.0F) -
+    COM_X * COM_Z * Mb * q3 * rk * t5 * t6 * 2.0F) - COM_Y * COM_Z * Mb * q2 *
+    rk * t5 * t6 * 6.0F) - COM_X * COM_Y * Mb * q1 * q2 * q3 * rk * t3 * 4.0F) -
+    COM_X * COM_Y * Mb * q1 * q2 * q3 * rk * t4 * 4.0F) + COM_X * COM_Z * Mb *
+    q1 * q2 * q4 * rk * t3 * 4.0F) + COM_X * COM_Z * Mb * q1 * q2 * q4 * rk * t4
+    * 4.0F) + COM_X * COM_Z * Mb * q1 * q2 * q4 * rk * t5 * 4.0F) + COM_X *
+    COM_Z * Mb * q1 * q2 * q4 * rk * t6 * 4.0F) - COM_Y * COM_Z * Mb * q1 * q3 *
+    q4 * rk * t4 * 8.0F) - COM_Y * COM_Z * Mb * q1 * q3 * q4 * rk * t5 * 8.0F) *
+    2.0F;
+  f0 = (((((((((((((((((((((((((((((t92 + t93) + t94) + t95) + t96) + t97) + t98)
+    + t99) + t100) + t101) + t102) + t103) - t104) - t105) - t106) - t107) -
+                     t108) - t109) + t110) - t111) - t112) - t113) + t114) -
+              t115) - t116) - t117) + t118) + t119) + t120) + Mb * q4 * rk * t8 *
+        t9 * 2.0F) + Mb * q4 * rk * t7 * t11 * 3.0F;
+  f1 = Mb * q4 * rk * t8 * t10 * 2.0F;
+  f2 = Mb * q4 * rk * t7;
+  f3 = Mb * q4 * rk * t10;
+  f4 = Mb * q4 * rk;
+  f5 = Mb * q4 * rk;
+  f6 = Mb * q4;
+  f7 = COM_X * COM_Y * Mb;
+  f8 = COM_X * COM_Y;
+  f9 = COM_X * COM_Z * Mb;
+  x[5] = Mb * g * rk * t32 * (((((((((((((((((((((((((((((((((((((((((((((f0 +
+    f1) - f2 * t13) - f3 * t11) - f4 * t9 * t13) + f5 * t10 * t13) + f6 * rk *
+    t12 * t13) + f7 * q1 * rk * t7) + f8 * Mb * q1 * rk * t9) + f9 * q2 * rk *
+    t7 * 3.0F) + COM_X * COM_Y * Mb * q1 * rk * t10) + COM_Y * COM_Z * Mb * q3 *
+    rk * t7) + COM_Y * COM_Z * Mb * q3 * rk * t9) + COM_X * COM_Z * Mb * q2 * rk
+    * t12 * 3.0F) + COM_Y * COM_Z * Mb * q3 * rk * t12) + Mb * q4 * rk * t3 * t4
+    * t8 * 4.0F) + Mb * q4 * rk * t3 * t5 * t8 * 6.0F) + Mb * q4 * rk * t3 * t6 *
+    t8 * 2.0F) - Mb * q4 * rk * t4 * t5 * t8 * 2.0F) + Mb * q4 * rk * t4 * t6 *
+    t8 * 2.0F) - Mb * q4 * rk * t3 * t5 * t13 * 2.0F) + Mb * q4 * rk * t4 * t5 *
+    t13 * 4.0F) + Mb * q4 * rk * t4 * t6 * t13 * 2.0F) + Mb * q4 * rk * t5 * t6 *
+    t13 * 4.0F) + Mb * q1 * q2 * q3 * rk * t5 * t8 * 4.0F) - Mb * q1 * q2 * q3 *
+    rk * t6 * t8 * 4.0F) + Mb * q1 * q2 * q3 * rk * t4 * t13 * 4.0F) + COM_X *
+    COM_Y * Mb * q1 * rk * t3 * t4 * 2.0F) + COM_X * COM_Y * Mb * q1 * rk * t3 *
+    t5 * 2.0F) - COM_X * COM_Y * Mb * q1 * rk * t4 * t6 * 2.0F) - COM_X * COM_Y *
+    Mb * q1 * rk * t5 * t6 * 10.0F) + COM_X * COM_Z * Mb * q2 * rk * t4 * t5 *
+    2.0F) + COM_Y * COM_Z * Mb * q3 * rk * t3 * t4 * 6.0F) + COM_X * COM_Z * Mb *
+    q2 * rk * t4 * t6 * 2.0F) + COM_Y * COM_Z * Mb * q3 * rk * t3 * t5 * 2.0F) -
+    COM_X * COM_Z * Mb * q2 * rk * t5 * t6 * 2.0F) - COM_Y * COM_Z * Mb * q3 *
+    rk * t4 * t6 * 2.0F) - COM_Y * COM_Z * Mb * q3 * rk * t5 * t6 * 6.0F) -
+    COM_X * COM_Y * Mb * q2 * q3 * q4 * rk * t3 * 4.0F) + COM_Y * COM_Z * Mb *
+    q1 * q2 * q4 * rk * t3 * 4.0F) + COM_X * COM_Y * Mb * q2 * q3 * q4 * rk * t4
+    * 4.0F) + COM_X * COM_Z * Mb * q1 * q3 * q4 * rk * t4 * 8.0F) + COM_Y *
+    COM_Z * Mb * q1 * q2 * q4 * rk * t4 * 4.0F) + COM_X * COM_Z * Mb * q1 * q3 *
+    q4 * rk * t5 * 8.0F) + COM_Y * COM_Z * Mb * q1 * q2 * q4 * rk * t5 * 4.0F) +
+    COM_Y * COM_Z * Mb * q1 * q2 * q4 * rk * t6 * 4.0F) * 2.0F;
+  f0 = (((((((((((((((((((((((((((((t40 + t41) + t42) + t43) + t44) + t45) + t46)
+    + t47) + t48) + t49) + t50) + t51) + t52) + t53) + t54) + t55) + t56) + t57)
+                   + t58) + t59) + t60) + t61) + t62) + t63) + t64) + t65) -
+           COM_Z * Jk * q2) - COM_Z * Mb * q2 * t2) - COM_Z * Mk * q2 * t2) - Mb
+        * q2 * rk * t7 * t8) + Mb * q2 * rk * t8 * t9;
+  f1 = Mb * q2 * rk * t8 * t10;
+  f2 = Mb * q2 * rk * t7 * t13;
+  f3 = Mb * q2 * rk * t8;
+  f4 = Mb * q2 * rk;
+  f5 = Mb * q2 * rk * t9;
+  f6 = Mb * q2 * rk;
+  f7 = COM_X * COM_Y * Mb;
+  f8 = COM_X * COM_Y;
+  f9 = COM_X * COM_Z;
+  t31 = COM_Y * COM_Z;
+  x[6] = Mb * g * rk * t32 * (((((((((((((((((((((((((((((((((((((((((((((f0 +
+    f1) + f2 * 2.0F) - f3 * t12) - f4 * t9 * t11) + f5 * t13 * 2.0F) + f6 * t11 *
+    t12 * 3.0F) - f7 * q3 * rk * t7) - f8 * Mb * q3 * rk * t9) - f9 * Mb * q4 *
+    rk * t7) - t31 * Mb * q1 * rk * t10 * 3.0F) - COM_X * COM_Y * Mb * q3 * rk *
+    t12) - COM_X * COM_Z * Mb * q4 * rk * t10) - COM_Y * COM_Z * Mb * q1 * rk *
+    t12 * 3.0F) - COM_X * COM_Z * Mb * q4 * rk * t12) + Mb * q2 * rk * t3 * t4 *
+    t8 * 2.0F) + Mb * q2 * rk * t3 * t6 * t8 * 4.0F) + Mb * q2 * rk * t4 * t6 *
+    t8 * 4.0F) - Mb * q2 * rk * t5 * t6 * t8 * 2.0F) + Mb * q2 * rk * t3 * t4 *
+    t13 * 2.0F) + Mb * q2 * rk * t3 * t5 * t13 * 4.0F) - Mb * q2 * rk * t3 * t6 *
+    t13 * 2.0F) + Mb * q2 * rk * t4 * t5 * t13 * 2.0F) + Mb * q2 * rk * t5 * t6 *
+    t13 * 6.0F) - Mb * q1 * q3 * q4 * rk * t3 * t8 * 4.0F) - Mb * q1 * q3 * q4 *
+    rk * t4 * t8 * 4.0F) - Mb * q1 * q3 * q4 * rk * t6 * t13 * 4.0F) + COM_X *
+    COM_Y * Mb * q3 * rk * t3 * t4 * 2.0F) - COM_Y * COM_Z * Mb * q1 * rk * t3 *
+    t4 * 2.0F) - COM_X * COM_Y * Mb * q3 * rk * t3 * t5 * 2.0F) - COM_Y * COM_Z *
+    Mb * q1 * rk * t3 * t6 * 2.0F) - COM_Y * COM_Z * Mb * q1 * rk * t4 * t5 *
+    2.0F) + COM_X * COM_Y * Mb * q3 * rk * t4 * t6 * 10.0F) - COM_X * COM_Z * Mb
+    * q4 * rk * t3 * t5 * 6.0F) - COM_X * COM_Y * Mb * q3 * rk * t5 * t6 * 2.0F)
+    - COM_X * COM_Z * Mb * q4 * rk * t4 * t5 * 2.0F) + COM_X * COM_Z * Mb * q4 *
+    rk * t4 * t6 * 6.0F) - COM_X * COM_Z * Mb * q4 * rk * t5 * t6 * 2.0F) +
+    COM_X * COM_Z * Mb * q1 * q2 * q3 * rk * t3 * 4.0F) + COM_X * COM_Z * Mb *
+    q1 * q2 * q3 * rk * t4 * 4.0F) - COM_X * COM_Y * Mb * q1 * q2 * q4 * rk * t5
+    * 4.0F) + COM_X * COM_Z * Mb * q1 * q2 * q3 * rk * t5 * 4.0F) - COM_X *
+    COM_Y * Mb * q1 * q2 * q4 * rk * t6 * 4.0F) + COM_X * COM_Z * Mb * q1 * q2 *
+    q3 * rk * t6 * 4.0F) + COM_Y * COM_Z * Mb * q2 * q3 * q4 * rk * t3 * 8.0F) +
+    COM_Y * COM_Z * Mb * q2 * q3 * q4 * rk * t6 * 8.0F) * -2.0F;
+  f0 = (((((((((((((((((((((((((((((t121 + t122) + t123) + t124) + t125) + t126)
+    - t127) - t128) - t129) - t130) - t131) - t132) - t133) - t134) - t135) +
+                      t136) - t137) - t138) - t139) - t140) - t141) + t142) -
+               t143) - t144) + t145) + t146) + t147) - COM_X * Jk * q1) - COM_Z *
+         Jk * q3) - COM_X * Mb * q1 * t2) - COM_Z * Mb * q3 * t2;
+  f1 = COM_X * Mk * q1 * t2;
+  f2 = COM_Z * Mk * q3;
+  f3 = Mb * q3 * rk * t8 * t9;
+  f4 = Mb * q3 * rk * t8;
+  f5 = Mb * q3 * rk;
+  f6 = Mb * q3;
+  f7 = Mb * q3;
+  f8 = Mb * q3;
+  f9 = COM_X * COM_Y;
+  x[7] = Mb * g * rk * t32 * (((((((((((((((((((((((((((((((((((((((((((((f0 -
+    f1) - f2 * t2) + f3 * 2.0F) + f4 * t10 * 2.0F) + f5 * t7 * t13) - f6 * rk *
+    t9 * t11) + f7 * rk * t9 * t13) - Mb * q3 * rk * t10 * t13) + f8 * rk * t11 *
+    t12 * 3.0F) - Mb * q3 * rk * t12 * t13) + f9 * Mb * q2 * rk * t7 * 3.0F) +
+    COM_X * COM_Z * Mb * q1 * rk * t7 * 3.0F) - COM_Y * COM_Z * Mb * q4 * rk *
+    t7) + COM_X * COM_Z * Mb * q1 * rk * t12 * 3.0F) + COM_Y * COM_Z * Mb * q4 *
+    rk * t9 * 3.0F) + Mb * q3 * rk * t3 * t4 * t8 * 4.0F) + Mb * q3 * rk * t3 *
+    t5 * t8 * 2.0F) - Mb * q3 * rk * t3 * t6 * t8 * 2.0F) + Mb * q3 * rk * t4 *
+    t5 * t8 * 2.0F) + Mb * q3 * rk * t4 * t6 * t8 * 6.0F) + Mb * q3 * rk * t3 *
+    t5 * t13 * 2.0F) + Mb * q3 * rk * t3 * t6 * t13 * 4.0F) - Mb * q3 * rk * t4 *
+    t6 * t13 * 2.0F) + Mb * q3 * rk * t5 * t6 * t13 * 4.0F) - Mb * q1 * q2 * q4 *
+    rk * t5 * t8 * 4.0F) + Mb * q1 * q2 * q4 * rk * t6 * t8 * 4.0F) + Mb * q1 *
+    q2 * q4 * rk * t3 * t13 * 4.0F) + COM_X * COM_Y * Mb * q2 * rk * t3 * t5 *
+    2.0F) + COM_X * COM_Z * Mb * q1 * rk * t3 * t5 * 2.0F) + COM_X * COM_Y * Mb *
+    q2 * rk * t3 * t6 * 6.0F) + COM_X * COM_Y * Mb * q2 * rk * t4 * t5 * 2.0F) +
+    COM_X * COM_Z * Mb * q1 * rk * t3 * t6 * 2.0F) + COM_X * COM_Y * Mb * q2 *
+    rk * t5 * t6 * 10.0F) - COM_X * COM_Z * Mb * q1 * rk * t5 * t6 * 2.0F) +
+    COM_Y * COM_Z * Mb * q4 * rk * t3 * t6 * 2.0F) - COM_Y * COM_Z * Mb * q4 *
+    rk * t4 * t5 * 2.0F) + COM_Y * COM_Z * Mb * q4 * rk * t5 * t6 * 6.0F) -
+    COM_Y * COM_Z * Mb * q1 * q2 * q3 * rk * t3 * 4.0F) - COM_Y * COM_Z * Mb *
+    q1 * q2 * q3 * rk * t4 * 4.0F) - COM_X * COM_Y * Mb * q1 * q3 * q4 * rk * t5
+    * 4.0F) + COM_X * COM_Z * Mb * q2 * q3 * q4 * rk * t3 * 8.0F) - COM_Y *
+    COM_Z * Mb * q1 * q2 * q3 * rk * t5 * 4.0F) + COM_X * COM_Y * Mb * q1 * q3 *
+    q4 * rk * t6 * 4.0F) - COM_Y * COM_Z * Mb * q1 * q2 * q3 * rk * t6 * 4.0F) +
+    COM_X * COM_Z * Mb * q2 * q3 * q4 * rk * t6 * 8.0F) * -2.0F;
+  i0 = 0;
+  i1 = 0;
+  i2 = 0;
+  for (i3 = 0; i3 < 8; i3++) {
+    dAcceleration_dq[i1 + (i0 << 2)] = x[i2];
+    i0++;
+    if (i0 > 1) {
+      i0 = 0;
+      i1++;
+    }
+
+    i2++;
+  }
+}
+
+//
+// File trailer for SteadyStateAcceleration_dq.cpp
+//
+// [EOF]
+//
diff --git a/KugleFirmware/Libraries/Modules/Estimators/VelocityEKF/MATLABCoder/SteadyStateAcceleration_dq.h b/KugleFirmware/Libraries/Modules/Estimators/VelocityEKF/MATLABCoder/SteadyStateAcceleration_dq.h
new file mode 100644
index 0000000..021e769
--- /dev/null
+++ b/KugleFirmware/Libraries/Modules/Estimators/VelocityEKF/MATLABCoder/SteadyStateAcceleration_dq.h
@@ -0,0 +1,30 @@
+//
+// Academic License - for use in teaching, academic research, and meeting
+// course requirements at degree granting institutions only.  Not for
+// government, commercial, or other organizational use.
+// File: SteadyStateAcceleration_dq.h
+//
+// MATLAB Coder version            : 4.0
+// C/C++ source code generated on  : 19-Nov-2018 11:57:59
+//
+#ifndef STEADYSTATEACCELERATION_DQ_H
+#define STEADYSTATEACCELERATION_DQ_H
+
+// Include Files
+#include <stddef.h>
+#include <stdlib.h>
+#include "rtwtypes.h"
+#include "VelocityEstimator_types.h"
+
+// Function Declarations
+extern void SteadyStateAcceleration_dq(float COM_X, float COM_Y, float COM_Z,
+  float Jk, float Mb, float Mk, float g, float q1, float q2, float q3, float q4,
+  float rk, float dAcceleration_dq[8]);
+
+#endif
+
+//
+// File trailer for SteadyStateAcceleration_dq.h
+//
+// [EOF]
+//
diff --git a/KugleFirmware/Libraries/Modules/Estimators/VelocityEKF/MATLABCoder/VelocityEstimator2.h b/KugleFirmware/Libraries/Modules/Estimators/VelocityEKF/MATLABCoder/VelocityEstimator2.h
new file mode 100644
index 0000000..ea2b639
--- /dev/null
+++ b/KugleFirmware/Libraries/Modules/Estimators/VelocityEKF/MATLABCoder/VelocityEstimator2.h
@@ -0,0 +1,34 @@
+//
+// Academic License - for use in teaching, academic research, and meeting
+// course requirements at degree granting institutions only.  Not for
+// government, commercial, or other organizational use.
+// File: VelocityEstimator2.h
+//
+// MATLAB Coder version            : 4.0
+// C/C++ source code generated on  : 25-Feb-2019 19:14:04
+//
+#ifndef VELOCITYESTIMATOR2_H
+#define VELOCITYESTIMATOR2_H
+
+// Include Files
+#include <stddef.h>
+#include <stdlib.h>
+#include "rtwtypes.h"
+#include "VelocityEstimator_types.h"
+
+// Function Declarations
+extern void VelocityEstimator2(const float X[2], const float P_prev[4], const
+  float EncoderDiffMeas[3], const float qQEKF[4], const float Cov_qQEKF[16],
+  const float qdotQEKF[4], float SamplePeriod, float TicksPrRev, float Jk, float
+  Mk, float rk, float Mb, float Jw, float rw, float l, float g, const float COM
+  [3], float CoR, float Var_COM, float eta_encoder, boolean_T
+  UseTiltForPrediction, boolean_T EstimateVelocityAtCoR, float X_out[2], float
+  P_out[4]);
+
+#endif
+
+//
+// File trailer for VelocityEstimator2.h
+//
+// [EOF]
+//
diff --git a/KugleFirmware/Libraries/Modules/PowerManagement/PowerManagement.cpp b/KugleFirmware/Libraries/Modules/PowerManagement/PowerManagement.cpp
index d8186f0..1f8c498 100644
--- a/KugleFirmware/Libraries/Modules/PowerManagement/PowerManagement.cpp
+++ b/KugleFirmware/Libraries/Modules/PowerManagement/PowerManagement.cpp
@@ -19,18 +19,49 @@
 #include "PowerManagement.h"
 #include "stm32h7xx_hal.h"
 #include "cmsis_os.h" // for processing task
+#include "SMBus.h"
+#include "Battery.h"
+#include "BatteryBoard.h"
+
 
 #include "Debug.h"
 
-PowerManagement::PowerManagement(IO& enable19V, IO& enable5V, Battery& bat1, Battery& bat2, PWM& powerLED, uint32_t powerManagementTaskPriority) : _powerManagementTaskHandle(0), _enable19V(enable19V), _enable5V(enable5V), _bat1(bat1), _bat2(bat2), _powerLED(powerLED)
+PowerManagement::PowerManagement(IO& enable19V, IO& enable5V, PWM& powerLED, uint32_t powerManagementTaskPriority, LSPC& com_, Timer& microsTimer_) : _powerManagementTaskHandle(0), _enable19V(enable19V), _enable5V(enable5V), _powerLED(powerLED), com(com_), microsTimer(microsTimer_)
 {
-	_ledMode = LEDMODE_PULSING;
+	_ledMode_pending = LEDMODE_PULSING;
 	_PulseValue = 0;
 	_PulseDirectionUp = true;
 
 	_powerMode = POWERMODE_OFF;
 	Enable(true, false); // start with 19V turned on and 5V off
 
+	osDelay(2000);
+
+	for(uint8_t i = 0;i<nSMBusses;i++)
+	{
+		smartBatteryBoard[i] = new BatteryBoard(SMBusHardwareResources[i],chargerCurrentLimit/nSMBusses);
+		smartBattery[i] = new Battery(SMBusHardwareResources[i], i);
+	}
+
+	powerManagmentStaticInfo.batteryAssamblyGettingOld = false;
+	powerManagmentStaticInfo.batteryAssamblyTotalCapacity = 0;
+
+	for(int i = 0;i<nSMBusses;i++){
+		// static values only requested at initialization!
+
+		batteryStaticInfo[i].serial_number = smartBattery[i]->getSerialNumber();
+		batteryStaticInfo[i].capacity = smartBattery[i]->getCapacity();
+		batteryStaticInfo[i].design_capacity = smartBattery[i]->getDesign_capacity();
+		batteryStaticInfo[i].power_supply_technology = smartBattery[i]->getPowerSupplyTechnology();
+		batteryStaticInfo[i].present = smartBattery[i]->checkPresent();
+		batteryStaticInfo[i].location = smartBattery[i]->getLocation();
+
+		powerManagmentStaticInfo.batteryAssamblyTotalCapacity = powerManagmentStaticInfo.batteryAssamblyTotalCapacity + batteryStaticInfo[i].capacity;
+		powerManagmentStaticInfo.batteryAssamblyGettingOld = (powerManagmentStaticInfo.batteryAssamblyGettingOld || smartBattery[i]->isBatteryGettingOld()); // not part of the standard ros battery msg. thus only included in the powermanagment info msg
+
+	}
+
+
 	xTaskCreate(PowerManagement::PowerManagementThread, (char *)"Power Management", POWER_MANAGEMENT_THREAD_STACK, (void*) this, powerManagementTaskPriority, &_powerManagementTaskHandle);
 }
 
@@ -38,6 +69,12 @@ PowerManagement::~PowerManagement()
 {
 	if (_powerManagementTaskHandle)
 		vTaskDelete(_powerManagementTaskHandle); // stop task
+
+	for(int i = 0;i<nSMBusses;i++)
+	{
+		if(smartBattery[i]) delete smartBattery[i];
+		if(smartBatteryBoard[i]) delete smartBatteryBoard[i];
+	}
 }
 
 void PowerManagement::Enable(bool enable19V, bool enable5V)
@@ -55,27 +92,7 @@ void PowerManagement::Enable(bool enable19V, bool enable5V)
 
 void PowerManagement::SetLEDmode(LEDmode_t ledMode)
 {
-	LEDmode_t prevLedMode = _ledMode;
-	_ledMode = ledMode;
-
-	/*if (_ledMode == ON) {
-		if (_powerLED)
-			_powerLED->SetRaw(POWER_LED_PWM_RANGE);
-	}
-	else if (_ledMode == OFF) {
-		if (_powerLED)
-			_powerLED->SetRaw(0);
-	}
-	else if (_ledMode == PULSING) {
-		if (prevLedMode == ON) {
-			_PulseValue = POWER_LED_PWM_RANGE;
-			_PulseDirectionUp = false; // down
-		} else if (prevLedMode == OFF) {
-			_PulseValue = POWER_LED_PWM_RANGE;
-			_PulseDirectionUp = true; // up
-		}
-		//vTaskResume(_powerManagementTaskHandle);
-	}*/
+	_ledMode_pending = ledMode;
 }
 
 void PowerManagement::SetPowerMode(PowerManagement::PowerMode_t powerMode)
@@ -114,6 +131,130 @@ void PowerManagement::CalibrateButtonInterrupt(void * params)
 }
 #endif
 
+void PowerManagement::SendBatteryInfo()
+{
+	lspc::MessageTypesToPC::powerManagment_info_t powerManagment_msg;
+	lspc::MessageTypesToPC::RawSensor_Battery_t battery_msg[nSMBusses];
+
+	for(int i = 0;i<nSMBusses;i++)
+	{
+
+		battery_msg[i].current = smartBattery[i]->getCurrent();
+		osDelay(5);
+		battery_msg[i].voltage = smartBattery[i]->getVoltage();
+		osDelay(5);
+		battery_msg[i].charge = smartBattery[i]->getCharge();
+		osDelay(5);
+		battery_msg[i].percentage = smartBattery[i]->getPercentage();
+		osDelay(5);
+
+		battery_msg[i].power_supply_status = smartBattery[i]->getPowerSupplyStatus();
+		osDelay(5);
+		battery_msg[i].power_supply_health = smartBattery[i]->getPowerSupplyHealth();
+		osDelay(5);
+
+		battery_msg[i].serial_number = batteryStaticInfo[i].serial_number;
+		battery_msg[i].capacity = batteryStaticInfo[i].capacity;
+		battery_msg[i].design_capacity = batteryStaticInfo[i].design_capacity;
+		battery_msg[i].power_supply_technology = batteryStaticInfo[i].power_supply_technology;
+		battery_msg[i].present = batteryStaticInfo[i].present;
+		battery_msg[i].location = batteryStaticInfo[i].location;
+
+
+		smartBatteryBoard[i]->SetCurrentLimitRAM(chargerCurrentLimit/nSMBusses);
+	}
+
+	float batteryAssamblyChargePercentage = 0;
+	float batteryAssamblyTotalCharge = 0;
+
+	// add check of charge current and correct if it does not fit the charger.
+	// consider if some of the above shold only be called once when the platform startsup? e.g. getLocation, getPowerSupplyTechnology, isBatteryGettingOld, serial_number etc
+	// add function to change gettingOldCycleCount
+	// add ros services to change appropriate values
+
+	for(int i = 0;i<nSMBusses;i++){
+		batteryAssamblyChargePercentage = batteryAssamblyChargePercentage + battery_msg[i].percentage;
+		batteryAssamblyTotalCharge = powerManagmentStaticInfo.batteryAssamblyTotalCharge + battery_msg[i].charge;
+	}
+	batteryAssamblyChargePercentage = batteryAssamblyChargePercentage/(float)nSMBusses;
+
+	if(batteryAssamblyChargePercentage<BATTERY_LOW_PERCENTAGE){
+		batteryLOW = true;
+		Debug::printf("battery low /n");
+	}
+	else{batteryLOW = false;}
+
+	powerManagment_msg.time = microsTimer.GetTime();
+	powerManagment_msg.batteryAssamblyGettingOld = powerManagmentStaticInfo.batteryAssamblyGettingOld;
+	powerManagment_msg.batteryAssamblyChargePercentage = batteryAssamblyChargePercentage;
+	powerManagment_msg.batteryAssamblyTotalCapacity = powerManagmentStaticInfo.batteryAssamblyTotalCapacity;
+	powerManagment_msg.batteryAssamblyTotalCharge = batteryAssamblyTotalCharge;
+	powerManagment_msg.nBatteries = nSMBusses;
+
+	// add additonal data processing
+	powerManagment_msg.power_supply_health = Battery::POWER_SUPPLY_HEALTH_UNKNOWN; // add some processing to get proper feedback
+	powerManagment_msg.power_supply_status = Battery::POWER_SUPPLY_STATUS_UNKNOWN; // add some processing to get proper feedback
+
+
+	// send msgs
+	com.TransmitAsync(lspc::MessageTypesToPC::powerManagment_info, (uint8_t *)&powerManagment_msg, sizeof(powerManagment_msg));
+
+	for(int i = 0;i<nSMBusses;i++)
+	{
+		com.TransmitAsync(lspc::MessageTypesToPC::RawSensor_Battery, (uint8_t *)&battery_msg[i], sizeof(battery_msg[0]));
+	}
+}
+
+
+void PowerManagement::BlinkLED(){
+
+	if (batteryLOW){
+		_ledMode = LEDMODE_BLINKING_FAST;
+	}
+	else{
+		_ledMode = _ledMode_pending;
+	}
+
+	if (_ledMode == PowerManagement::LEDMODE_PULSING) {
+		if (_PulseDirectionUp && _PulseValue < POWER_LED_PWM_RANGE)
+			_PulseValue++;
+		else if (!_PulseDirectionUp && _PulseValue > 0)
+			_PulseValue--;
+		else
+			_PulseDirectionUp = !_PulseDirectionUp;
+
+		_powerLED.SetRaw(_PulseValue);
+	}
+	else if (_ledMode == PowerManagement::LEDMODE_BLINKING) {
+		_PulseValue++;
+		if (_PulseValue > POWER_LED_PWM_RANGE)
+			_PulseValue = 0;
+
+		if (_PulseValue > POWER_LED_PWM_RANGE/2)
+			_powerLED.SetRaw(POWER_LED_PWM_RANGE);
+		else
+			_powerLED.SetRaw(0);
+	}
+	else if (_ledMode == PowerManagement::LEDMODE_BLINKING_FAST) {
+		_PulseValue++;
+		if (_PulseValue > POWER_LED_PWM_RANGE/2)
+			_PulseValue = 0;
+
+		if (_PulseValue > POWER_LED_PWM_RANGE/4)
+			_powerLED.SetRaw(POWER_LED_PWM_RANGE);
+		else
+			_powerLED.SetRaw(0);
+	}
+	else if (_ledMode == PowerManagement::LEDMODE_ON) {
+		_powerLED.SetRaw(POWER_LED_PWM_RANGE);
+		_PulseValue = POWER_LED_PWM_RANGE;
+	}
+	else if (_ledMode == PowerManagement::LEDMODE_OFF) {
+		_powerLED.SetRaw(0);
+		_PulseValue = 0;
+	}
+}
+
 void PowerManagement::PowerManagementThread(void * pvParameters)
 {
 	PowerManagement * pm = (PowerManagement *)pvParameters;
@@ -121,49 +262,25 @@ void PowerManagement::PowerManagementThread(void * pvParameters)
 	/* Consider to implement this as a high priority 'watchdog task' (referred to as a 'check' task in all the official demos) that monitors how the cycle counters of each task to ensure they are cycling as expected */
 	/* This task could possibly also poll for the real time stats - https://www.freertos.org/a00021.html#vTaskGetRunTimeStats */
 
+	uint16_t batteryPollingRateCounter = 0;
+	uint16_t batteryPollingRateCounterMax = BATTERY_POLLING_DT / POWER_MANAGMENT_THREAD_DT;
+
 	while (1) {
 		/*if (pm->_ledMode != PULSING)
 			vTaskSuspend(NULL); // suspend current thread - this could also be replaced by semaphore-based waiting (flagging)
 		*/
 
-		if (pm->_ledMode == PowerManagement::LEDMODE_PULSING) {
-			if (pm->_PulseDirectionUp && pm->_PulseValue < POWER_LED_PWM_RANGE)
-				pm->_PulseValue++;
-			else if (!pm->_PulseDirectionUp && pm->_PulseValue > 0)
-				pm->_PulseValue--;
-			else
-				pm->_PulseDirectionUp = !pm->_PulseDirectionUp;
-			pm->_powerLED.SetRaw(pm->_PulseValue);
-		}
-		else if (pm->_ledMode == PowerManagement::LEDMODE_BLINKING) {
-			pm->_PulseValue++;
-			if (pm->_PulseValue > POWER_LED_PWM_RANGE)
-				pm->_PulseValue = 0;
-
-			if (pm->_PulseValue > POWER_LED_PWM_RANGE/2)
-				pm->_powerLED.SetRaw(POWER_LED_PWM_RANGE);
-			else
-				pm->_powerLED.SetRaw(0);
-		}
-		else if (pm->_ledMode == PowerManagement::LEDMODE_BLINKING_FAST) {
-			pm->_PulseValue++;
-			if (pm->_PulseValue > POWER_LED_PWM_RANGE/2)
-				pm->_PulseValue = 0;
-
-			if (pm->_PulseValue > POWER_LED_PWM_RANGE/4)
-				pm->_powerLED.SetRaw(POWER_LED_PWM_RANGE);
-			else
-				pm->_powerLED.SetRaw(0);
-		}
-		else if (pm->_ledMode == PowerManagement::LEDMODE_ON) {
-			pm->_powerLED.SetRaw(POWER_LED_PWM_RANGE);
-			pm->_PulseValue = POWER_LED_PWM_RANGE;
-		}
-		else if (pm->_ledMode == PowerManagement::LEDMODE_OFF) {
-			pm->_powerLED.SetRaw(0);
-			pm->_PulseValue = 0;
+		// Blinking led
+		pm->BlinkLED();
+
+		// Battery Data Polling
+		if (batteryPollingRateCounter==batteryPollingRateCounterMax){
+			pm->SendBatteryInfo();
+			batteryPollingRateCounter = 0;
 		}
+		batteryPollingRateCounter++;
 
-		osDelay(6);
+		// Pause task
+		osDelay(POWER_MANAGMENT_THREAD_DT); // ms delay
 	}
 }
diff --git a/KugleFirmware/Libraries/Modules/PowerManagement/PowerManagement.h b/KugleFirmware/Libraries/Modules/PowerManagement/PowerManagement.h
index cc408a3..b667fe7 100644
--- a/KugleFirmware/Libraries/Modules/PowerManagement/PowerManagement.h
+++ b/KugleFirmware/Libraries/Modules/PowerManagement/PowerManagement.h
@@ -26,16 +26,23 @@
 #include "PWM.h"
 #include "Battery.h"
 #include "Parameters.h"
+#include "Timer.h"
+#include "SMBus.h"
+
+#include "BatteryBoard.h"
+
+// CONFIGURATIONS
+#define POWER_LED_PWM_FREQUENCY 	200
+#define POWER_LED_PWM_RANGE			100
+#define POWER_MANAGMENT_THREAD_DT	10 		// ms
+#define BATTERY_POLLING_DT			1000  	// ms  choose something that adds up to POWER_MANAGMENT_THREAD_DT
 
-#define POWER_LED_PWM_FREQUENCY 200
-#define POWER_LED_PWM_RANGE		100
 
 class PowerManagement
 {
-	private:
-		const uint32_t POWER_MANAGEMENT_THREAD_STACK = 256;
-
 	public:
+		bool batteryAssamblyGettingOld;
+
 		typedef enum LEDmode_t {
 			LEDMODE_ON,
 			LEDMODE_OFF,
@@ -50,35 +57,58 @@ class PowerManagement
 			POWERMODE_5V_ONLY = 2
 		} PowerMode_t;
 
-	public:
-		PowerManagement(IO& enable19V, IO& enable5V, Battery& bat1, Battery& bat2, PWM& powerLED, uint32_t powerManagementTaskPriority);
-		~PowerManagement();
-
-		void SetPowerMode(PowerMode_t powerMode);
-		PowerMode_t GetPowerMode();
-		void SetLEDmode(LEDmode_t ledMode);
-
 	private:
+		// constants that can be changed through rosservice!
+        uint16_t chargerCurrentLimit = 7800;	// mA - actually 8000mA however set a bit lower to be safe!
+		float BATTERY_LOW_PERCENTAGE = 0.05;	// a number between 0 and 1!!!
+
 		IO& _enable19V; // Devices on 19V bus: LiDAR, PC (Intel NUC or Jetson) and Motors
 		IO& _enable5V;  // Devices on 5V bus: Xsens IMU, USB Hub
-		Battery& _bat1;
-		Battery& _bat2;
 		PWM& _powerLED;
+		LSPC& com;
+		Timer& microsTimer;
 
 		PowerMode_t _powerMode;
 
 		uint8_t _PulseValue;
 		bool _PulseDirectionUp;
 		LEDmode_t _ledMode;
+		LEDmode_t _ledMode_pending;
+		bool batteryLOW = false;
 
+		// task params
+		const uint32_t POWER_MANAGEMENT_THREAD_STACK = 256;
 		TaskHandle_t _powerManagementTaskHandle;
 
+		// SMBus constants
+		// According to the SMBus v 1.1. standard (which the current batteries complies to)it is required
+		// to have either a Smart Battery Selector and/or the Smart Battery System Manager to have multiple
+		// batteries on the same bus. Since such devices was not available on the system at the time of
+		// writing this code 2 busses was used for the duel battery system.
+		static const uint8_t nSMBusses = 2;
+		SMBus::port_t SMBusHardwareResources[nSMBusses] = {SMBus::PORT_I2C2, SMBus::PORT_I2C4};
+		Battery * smartBattery[nSMBusses];
+		BatteryBoard * smartBatteryBoard[nSMBusses];
+
+		lspc::MessageTypesToPC::powerManagment_info_t powerManagmentStaticInfo;
+		lspc::MessageTypesToPC::RawSensor_Battery_t batteryStaticInfo[nSMBusses];
+
+	public:
+		PowerManagement(IO& enable19V, IO& enable5V, PWM& powerLED, uint32_t powerManagementTaskPriority, LSPC& com_, Timer& microsTimer_);
+		~PowerManagement();
+
+		void SetPowerMode(PowerMode_t powerMode);
+		PowerMode_t GetPowerMode();
+		void SetLEDmode(LEDmode_t ledMode);
+
 
 	private:
 		static void PowerButtonInterrupt(void * params);
 		static void ResetButtonInterrupt(void * params);
 		static void CalibrateButtonInterrupt(void * params);
 		static void PowerManagementThread(void * pvParameters);
+		void SendBatteryInfo();
+		void BlinkLED();
 
 		void Enable(bool enable19V, bool enable5V);
 		void ButtonHandler();
diff --git a/KugleFirmware/Libraries/Periphirals/SMBus/SMBus.cpp b/KugleFirmware/Libraries/Periphirals/SMBus/SMBus.cpp
index beed597..539e8d0 100644
--- a/KugleFirmware/Libraries/Periphirals/SMBus/SMBus.cpp
+++ b/KugleFirmware/Libraries/Periphirals/SMBus/SMBus.cpp
@@ -42,8 +42,12 @@ SMBus::SMBus(port_t port, uint8_t devAddr, uint32_t frequency)
 	ConfigurePeripheral();
 }
 
-SMBus::SMBus(port_t port, uint8_t devAddr) : SMBus(port, devAddr, SMBUS_DEFAULT_FREQUENCY)
+SMBus::SMBus(port_t port, uint8_t devAddr)// : SMBus(port, devAddr, SMBUS_DEFAULT_FREQUENCY)
 {
+	_devAddr = devAddr << 1;
+	//_devAddr = devAddr;
+	InitPeripheral(port, SMBUS_DEFAULT_FREQUENCY);
+	ConfigurePeripheral();
 }
 
 SMBus::~SMBus()
@@ -213,75 +217,217 @@ void SMBus::InitPeripheral(port_t port, uint32_t frequency)
 
 void SMBus::ConfigurePeripheral()
 {
-	if (!_hRes) return;
-	if (!_hRes->configured) { // only configure periphiral once
-		switch (_hRes->port) {
-			case PORT_I2C2:
-				_hRes->handle.Instance = I2C2;
-				break;
-			case PORT_I2C4:
-				_hRes->handle.Instance = I2C4;
-				break;
-			default:
+	if (!_hRes) { // pointer to hardware resource already exists!
+		return;
+	}
+	else{
+		if (!_hRes->configured) { // only configure periphiral once
+			switch (_hRes->port) {
+				case PORT_I2C2:
+					_hRes->handle.Instance = I2C2;
+					break;
+				case PORT_I2C4:
+					_hRes->handle.Instance = I2C4;
+					break;
+				default:
+					_hRes = 0;
+					ERROR("Undefined SMBus port");
+					return;
+			}
+
+			_hRes->handle.Init.OwnAddress1 = SMBUS_MASTER_ADDRESS;
+			_hRes->handle.Init.AnalogFilter = SMBUS_ANALOGFILTER_ENABLE;
+			_hRes->handle.Init.AddressingMode = SMBUS_ADDRESSINGMODE_7BIT;
+			_hRes->handle.Init.DualAddressMode = SMBUS_DUALADDRESS_DISABLE;
+			_hRes->handle.Init.OwnAddress2 = 0;
+			_hRes->handle.Init.OwnAddress2Masks = SMBUS_OA2_NOMASK;
+			_hRes->handle.Init.GeneralCallMode = SMBUS_GENERALCALL_DISABLE;
+			_hRes->handle.Init.NoStretchMode = SMBUS_NOSTRETCH_DISABLE;
+			_hRes->handle.Init.PacketErrorCheckMode = SMBUS_PEC_ENABLE; //SMBUS_PEC_DISABLE;
+			_hRes->handle.Init.PeripheralMode = SMBUS_PERIPHERAL_MODE_SMBUS_HOST;
+			_hRes->handle.Init.SMBusTimeout = 0x000084C4;
+
+			//_hRes->handle.Init.Timing = 0x10C0ECFF;
+			/* Determine I2C timing configuration  (see reference manual page 1015)
+			    tI2CCLK = 1/100Mhz = 10 ns
+
+				PRESC = 0001 = 1
+				tPRESC = (PRESC+1)*tI2CCLK = 20 ns
+
+				SCLDEL = 1100 = 12
+				tSCLDEL = (SCLDEL+1)*tPRESC = 260 ns     (Data setup time)
+
+				SDADEL = 0000 = 0
+				tSDADEL = (SDADEL+1)*tPRESC = 20 ns      (Data hold time)
+
+				SCLH = 11101100 = 236
+				tSCLH = (SCLH+1)*tPRESC = 4740 ns        (SCL high period)
+
+				SCLL = 11111111 = 255
+				tSCLL = (SCLL+1)*tPRESC = 5120 ns        (SCL low period)
+
+				Period = 4740 + 5120 = 9860 ns
+				I2C frequency = 101419,88 Hz
+			*/
+
+			/*uint32_t I2C_Clock = HAL_RCC_GetPCLK1Freq(); // assuming all I2C periphirals to be configured with the same clock frequency running at the same frequency as APB1
+			uint32_t timingParam = 0x10C00000; // set pre-scaler, data setup and data hold time as listed above
+			uint8_t PRESC =  (timingParam & 0xF0000000) >> 28;
+			uint8_t SCLDEL = (timingParam & 0x00F00000) >> 20;
+			uint8_t SDADEL = (timingParam & 0x000F0000) >> 16;
+			uint16_t period = ((I2C_Clock/(PRESC+1)) / _hRes->frequency) / 2;
+			uint16_t periodHigh = period - (SCLDEL+1) - (SDADEL+1);
+			uint16_t periodLow = period;
+			_hRes->handle.Init.Timing = timingParam | (periodHigh << 8) | periodLow;
+			*/
+
+			// reference: page 1994 in "RM0433 Reference manual STM32H743/753 and STM32H750 advanced ARM®-based 32-bit MCUs"
+
+			/*uint32_t I2CCLK = HAL_RCC_GetPCLK1Freq(); // assuming all I2C periphirals to be configured with the same clock frequency running at the same frequency as APB1
+			uint32_t PRESC = (I2CCLK/(uint32_t)_hRes->frequency) - 1;
+			if(PRESC>15)
+			{
+				PRESC = 15;
+			} // only 4 bit can be stored in the PRESC reg
+
+			uint32_t timingParam =  PRESC << 28;
+			uint8_t SCLDEL = 0x01; // 0x0C;
+			uint8_t SDADEL = 0x01; // 0x00;
+			timingParam = timingParam | SCLDEL << 20;
+			timingParam = timingParam | SDADEL << 16;
+
+			uint32_t period = ((I2CCLK/( (uint32_t)PRESC+1)) / (uint32_t)_hRes->frequency) /2;
+			uint16_t SCLH = (uint16_t)period - ((uint16_t)SCLDEL+1) - ((uint16_t)SDADEL+1);
+			uint16_t SCLL = (uint16_t)period;
+			_hRes->handle.Init.Timing = timingParam | (SCLH << 8) | SCLL;
+			*/
+
+			uint32_t I2CCLK = HAL_RCC_GetPCLK1Freq(); // assuming all I2C periphirals to be configured with the same clock frequency running at the same frequency as APB1
+			uint8_t SCLDEL = 0x01; // hardcoded values! do they always work?
+			uint8_t SDADEL = 0x00; // hardcoded values! do they always work?
+
+			uint32_t PRESC = 0;
+			uint32_t SCLH = 0;
+			uint32_t SCLL = 0;
+
+			while(1){ // iterate until the lowest possible PRESC how been chosen that allows  appropriate values of SCLH and SCLL
+				uint32_t period = ((I2CCLK/( (uint32_t)PRESC+1)) / (uint32_t)_hRes->frequency) /2;
+				SCLH = (uint16_t)period - ((uint16_t)SCLDEL+1) - ((uint16_t)SDADEL+1);
+				SCLL = (uint16_t)period;
+
+				if ((SCLH>255)||(SCLL>255)){PRESC++;}
+				else{break;}
+
+				if(PRESC>15){ // error - only 4 bit can be stored in the PRESC reg
+					ERROR("The I2CCLK frequency is to high compared to the requsted clk.");
+					break;
+				}
+			}
+
+			// Compose the value of the timing Register
+			uint32_t timingParam =  PRESC << 28;
+			timingParam = timingParam | SCLDEL << 20;
+			timingParam = timingParam | SDADEL << 16;
+			timingParam = timingParam | (SCLH << 8) | SCLL;
+			_hRes->handle.Init.Timing = timingParam;
+			/*
+			//uint32_t timingParam = 0x20C00000;
+			uint8_t PRESC2 =  (timingParam & 0xF0000000) >> 28;
+			uint8_t SCLDEL2 = (timingParam & 0x00F00000) >> 20;
+			uint8_t SDADEL2 = (timingParam & 0x000F0000) >> 16;
+
+			osDelay(2000);
+			Debug::printf("SMBus timing - timingParam is: %X \n", timingParam);
+			Debug::printf("SMBus timing - I2C_Clock is: %lu \n", I2CCLK);
+			Debug::printf("SMBus timing - PRESC is: %u \n", PRESC);
+			Debug::printf("SMBus timing - PRESC2 is: %u \n", PRESC2);
+			Debug::printf("SMBus timing - SCLDEL2 is: %u \n", SCLDEL2);
+			Debug::printf("SMBus timing - SDADEL2 is: %u \n", SDADEL2);
+			Debug::printf("SMBus timing - _hRes->frequency is: %u \n", _hRes->frequency);
+			Debug::printf("SMBus timing - SCLH2 is: %u \n", SCLH);
+			Debug::printf("SMBus timing - period is: %lu \n", period);
+			Debug::printf("SMBus timing - SCLL2 is: %u \n", SCLL);
+
+			// check if the parameters complies with the SMBus Standard!
+			uint32_t tI2CCLK = 1/I2C_Clock;
+			uint32_t tPRESC = (PRESC+1)*I2C_Clock;
+			uint32_t tSCLDEL = (SCLDEL+1) * tPRESC;
+			uint32_t tSDADEL = SDADEL * tPRESC;
+			uint32_t tSCLH = (SCLH+1) * tPRESC;
+			uint32_t tSCLL = (SCLL+1) * tPRESC;
+
+			tPRESC = (PRESC+1) * tI2CCLK
+			PRESC = tPRESC/tI2CCLK - 1;
+			PRESC = (1/_hRes->frequency)/(1/I2CCLK) - 1;
+			PRESC = (1/_hRes->frequency)/(1/I2CCLK) - 1;
+			PRESC = I2CCLK/_hRes->frequency - 1;
+
+			tSCLH = (SCLH+1) * tPRESC
+			tPRESC/2 = (SCLH+1) * tPRESC
+			tPRESC/(2*tPRESC) = (SCLH+1)
+
+			uint32_t tSCL = tSYNC1 + tSYNC2 + (( (SCLH+1) + (SCLL+1) ) * (PRESC+1) * tI2CCLK);
+
+			if (tSCL > 10000) // ns - coresponding to 100kHz
+			{
+				Debug::printf("SMBus timing not complying to standard - tSCL is: %d ns\n", tSCL);
+			}
+
+			if ( (tf_max + tHD_DAT_min - ((DNF +3) * tI2CCLK)) / ((PRESC +1) * tI2CCLK ) <= SDADEL )
+	*/
+
+			if (HAL_SMBUS_Init(&_hRes->handle) != HAL_OK)
+			{
 				_hRes = 0;
-				ERROR("Undefined SMBus port");
+				ERROR("Could not initialize SMBus port");
 				return;
-		}
+			}
 
-		_hRes->handle.Init.OwnAddress1 = SMBUS_MASTER_ADDRESS;
-		_hRes->handle.Init.AnalogFilter = SMBUS_ANALOGFILTER_ENABLE;
-		_hRes->handle.Init.AddressingMode = SMBUS_ADDRESSINGMODE_7BIT;
-		_hRes->handle.Init.DualAddressMode = SMBUS_DUALADDRESS_DISABLE;
-		_hRes->handle.Init.OwnAddress2 = 0;
-		_hRes->handle.Init.OwnAddress2Masks = SMBUS_OA2_NOMASK;
-		_hRes->handle.Init.GeneralCallMode = SMBUS_GENERALCALL_DISABLE;
-		_hRes->handle.Init.NoStretchMode = SMBUS_NOSTRETCH_DISABLE;
-		_hRes->handle.Init.PacketErrorCheckMode = SMBUS_PEC_DISABLE;
-		_hRes->handle.Init.PeripheralMode = SMBUS_PERIPHERAL_MODE_SMBUS_HOST;
-		_hRes->handle.Init.SMBusTimeout = 0x000084C4;
-
-		//_hRes->handle.Init.Timing = 0x10C0ECFF;
-		/* Determine I2C timing configuration  (see reference manual page 1015)
-		    tI2CCLK = 1/100Mhz = 10 ns
-
-			PRESC = 0001 = 1
-			tPRESC = (PRESC+1)*tI2CCLK = 20 ns
-
-			SCLDEL = 1100 = 12
-			tSCLDEL = (SCLDEL+1)*tPRESC = 260 ns     (Data setup time)
-
-			SDADEL = 0000 = 0
-			tSDADEL = (SDADEL+1)*tPRESC = 20 ns      (Data hold time)
-
-			SCLH = 11101100 = 236
-			tSCLH = (SCLH+1)*tPRESC = 4740 ns        (SCL high period)
-
-			SCLL = 11111111 = 255
-			tSCLL = (SCLL+1)*tPRESC = 5120 ns        (SCL low period)
-
-			Period = 4740 + 5120 = 9860 ns
-			I2C frequency = 101419,88 Hz
-		*/
-
-		uint32_t I2C_Clock = HAL_RCC_GetPCLK1Freq(); // assuming all I2C periphirals to be configured with the same clock frequency running at the same frequency as APB1
-		uint32_t timingParam = 0x10C00000; // set prescaler, data setup and data hold time as listed above
-		uint8_t PRESC =  (timingParam & 0xF0000000) >> 28;
-		uint8_t SCLDEL = (timingParam & 0x00F00000) >> 20;
-		uint8_t SDADEL = (timingParam & 0x000F0000) >> 16;
-		uint16_t period = ((I2C_Clock/(PRESC+1)) / _hRes->frequency) / 2;
-		uint16_t periodHigh = period - (SCLDEL+1) - (SDADEL+1);
-		uint16_t periodLow = period;
-		_hRes->handle.Init.Timing = timingParam | (periodHigh << 8) | periodLow;
-
-		if (HAL_SMBUS_Init(&_hRes->handle) != HAL_OK)
-		{
-			_hRes = 0;
-			ERROR("Could not initialize SMBus port");
-			return;
+
+			uint32_t Trials = 5;
+			uint32_t Timeout = 100; //ms??
+			uint8_t tmp = HAL_SMBUS_IsDeviceReady(&_hRes->handle, _devAddr, Trials, Timeout);
+			if (tmp != HAL_OK)
+			{
+				Debug::printf("SMBus device with devAddr 0x%X not ready \n", _devAddr>>1);
+				return;
+			}
+		}
+		else{
+			Debug::printf("devAddr: 0x%X Entered Else \n", _devAddr>>1);
+
+			uint32_t Trials = 5;
+			uint32_t Timeout = 100; //ms??
+			uint8_t tmp = HAL_SMBUS_IsDeviceReady(&_hRes->handle, _devAddr, Trials, Timeout);
+			if (tmp != HAL_OK)
+			{
+				ERROR("SMBus device not ready after 5 trial");
+				return;
+			}
 		}
 	}
 }
 
+bool SMBus::devicePresent(){
+	if (!_hRes) {return false;}
+	else{
+		xSemaphoreTake( _hRes->resourceSemaphore, ( TickType_t ) portMAX_DELAY ); // take hardware resource
+
+		if (uxSemaphoreGetCount(_hRes->transmissionFinished)) // semaphore is available to be taken - which it should not be at this state before starting the transmission, since we use the semaphore for flagging the finish transmission event
+			xSemaphoreTake( _hRes->transmissionFinished, ( TickType_t ) portMAX_DELAY ); // something incorrect happened, as the transmissionFinished semaphore should always be taken before a transmission starts
+
+		uint32_t Trials = 1;
+		uint32_t Timeout = 100; //ms??
+		uint8_t tmp = HAL_SMBUS_IsDeviceReady(&_hRes->handle, _devAddr, Trials, Timeout);
+
+		xSemaphoreGive( _hRes->resourceSemaphore ); // give hardware resource back
+
+		if (tmp != HAL_OK){return false;}
+		else{return true;}
+	}
+}
+
+
 void SMBus::Write(uint8_t reg, uint8_t value)
 {
 	Write(reg, &value, 1);
@@ -289,31 +435,33 @@ void SMBus::Write(uint8_t reg, uint8_t value)
 
 void SMBus::Write(uint8_t reg, uint8_t * buffer, uint8_t writeLength)
 {
-	if (!_hRes) return;
+	//if (!_hRes) return;
+	if(!devicePresent()) return;
 	xSemaphoreTake( _hRes->resourceSemaphore, ( TickType_t ) portMAX_DELAY ); // take hardware resource
 
 	if (uxSemaphoreGetCount(_hRes->transmissionFinished)) // semaphore is available to be taken - which it should not be at this state before starting the transmission, since we use the semaphore for flagging the finish transmission event
-		xSemaphoreTake( _hRes->transmissionFinished, ( TickType_t ) portMAX_DELAY ); // something incorrect happened, as the transmissionFinished semaphore should always be taken before a transmission starts
+		    xSemaphoreTake( _hRes->transmissionFinished, ( TickType_t ) portMAX_DELAY ); // something incorrect happened, as the transmissionFinished semaphore should always be taken before a transmission starts
 
 	uint8_t * txBuffer = (uint8_t *)pvPortMalloc(writeLength+1);
 
-	if (!txBuffer) return;
+	if (!txBuffer)
+	{
+		ERROR("txBuffer not allocated");
+		return;
+	}
 
 	txBuffer[0] = reg;
 	memcpy(&txBuffer[1], buffer, writeLength);
 
-	uint32_t XferOptions = SMBUS_OTHER_FRAME_NO_PEC;
-
-	if (HAL_SMBUS_Master_Transmit_IT(&_hRes->handle, (uint16_t)_devAddr, buffer, writeLength, XferOptions) == HAL_OK)
+	uint32_t XferOptions = SMBUS_FIRST_AND_LAST_FRAME_WITH_PEC;// SMBUS_FIRST_AND_LAST_FRAME_NO_PEC; //SMBUS_FIRST_AND_LAST_FRAME_WITH_PEC;
+	if (HAL_SMBUS_Master_Transmit_IT(&_hRes->handle, (uint16_t)_devAddr, txBuffer, writeLength+2, XferOptions) == HAL_OK)
 	{
 		// Wait for the transmission to finish
 		xSemaphoreTake( _hRes->transmissionFinished, ( TickType_t ) portMAX_DELAY );
 	} else {
-		DEBUG("Failed SMBus transmission");
+		ERROR("Failed SMBus transmission");
 	}
 
-	//uint32_t errCode = HAL_SMBUS_GetError(&_hRes->handle);
-
 	vPortFree(txBuffer);
 
 	xSemaphoreGive( _hRes->resourceSemaphore ); // give hardware resource back
@@ -328,39 +476,133 @@ uint8_t SMBus::Read(uint8_t reg)
 
 void SMBus::Read(uint8_t reg, uint8_t * buffer, uint8_t readLength)
 {
-	if (!_hRes) return;
+	/*if (!_hRes)
+	{
+		ERROR("SMbus _hRes not initialised");
+		return;
+	}*/
+
+	if(!devicePresent()) return;
+
 	xSemaphoreTake( _hRes->resourceSemaphore, ( TickType_t ) portMAX_DELAY ); // take hardware resource
 
 	if (uxSemaphoreGetCount(_hRes->transmissionFinished)) // semaphore is available to be taken - which it should not be at this state before starting the transmission, since we use the semaphore for flagging the finish transmission event
+	{
 		xSemaphoreTake( _hRes->transmissionFinished, ( TickType_t ) portMAX_DELAY ); // something incorrect happened, as the transmissionFinished semaphore should always be taken before a transmission starts
+	}
 
-	uint32_t XferOptions = SMBUS_OTHER_FRAME_NO_PEC;
+	uint32_t stateCode;
+	uint32_t errCode;
 
 	// Write register address
-	if (HAL_SMBUS_Master_Transmit_IT(&_hRes->handle, (uint16_t)_devAddr, &reg, 1, XferOptions) == HAL_OK)
+	uint32_t XferOptions = SMBUS_FIRST_FRAME;//SMBUS_OTHER_FRAME_NO_PEC;
+	uint8_t tmp = HAL_SMBUS_Master_Transmit_IT(&_hRes->handle, (uint16_t)_devAddr, &reg, 1, XferOptions);
+	if (tmp == HAL_OK)
 	{
 		// Wait for the transmission to finish
 		xSemaphoreTake( _hRes->transmissionFinished, ( TickType_t ) portMAX_DELAY );
 	} else {
-		DEBUG("Failed SMBus transmission");
+		Debug::printf("Failed SMBus transmission!");
+		stateCode = HAL_SMBUS_GetState(&_hRes->handle);
+		errCode = HAL_SMBUS_GetError(&_hRes->handle);
+		Debug::printf("stateCode: %d     errCode: %d\n ",stateCode,errCode);
 	}
 
-	//uint32_t errCode = HAL_SMBUS_GetError(&_hRes->handle);
 
 	// Read data requested from register address
-	if (HAL_SMBUS_Master_Receive_IT(&_hRes->handle, (uint16_t)_devAddr, buffer, readLength, XferOptions) == HAL_OK)
+	XferOptions = SMBUS_LAST_FRAME_NO_PEC; //SMBUS_LAST_FRAME_WITH_PEC;//SMBUS_OTHER_FRAME_NO_PEC;
+	tmp = HAL_SMBUS_Master_Receive_IT(&_hRes->handle, (uint16_t)_devAddr, buffer, readLength, XferOptions);
+	if (tmp == HAL_OK)
+	{
+		// Wait for the the result
+		xSemaphoreTake( _hRes->transmissionFinished, ( TickType_t ) portMAX_DELAY );
+	} else {
+		Debug::printf("Failed SMBus reception!");
+		stateCode = HAL_SMBUS_GetState(&_hRes->handle);
+		errCode = HAL_SMBUS_GetError(&_hRes->handle);
+		Debug::printf("stateCode: %d     errCode: %d\n ",stateCode,errCode);
+	}
+
+
+	xSemaphoreGive( _hRes->resourceSemaphore ); // give hardware resource back
+}
+
+
+int16_t SMBus::blockRead(uint8_t reg, uint8_t * buffer, uint8_t bufferLength)
+{
+	// returns -1 if error!
+
+	if (!_hRes)
+	{
+		ERROR("SMbus _hRes not initialised");
+		return -1;
+	}
+
+	xSemaphoreTake( _hRes->resourceSemaphore, ( TickType_t ) portMAX_DELAY ); // take hardware resource
+
+	if (uxSemaphoreGetCount(_hRes->transmissionFinished)) // semaphore is available to be taken - which it should not be at this state before starting the transmission, since we use the semaphore for flagging the finish transmission event
+	{
+		xSemaphoreTake( _hRes->transmissionFinished, ( TickType_t ) portMAX_DELAY ); // something incorrect happened, as the transmissionFinished semaphore should always be taken before a transmission starts
+	}
+
+
+	uint32_t stateCode = HAL_SMBUS_GetState(&_hRes->handle);
+	uint32_t errCode = HAL_SMBUS_GetError(&_hRes->handle);
+
+	// Write register address
+	uint32_t XferOptions = SMBUS_FIRST_FRAME;//SMBUS_OTHER_FRAME_NO_PEC;
+	uint8_t tmp = HAL_SMBUS_Master_Transmit_IT(&_hRes->handle, (uint16_t)_devAddr, &reg, 1, XferOptions);
+	if (tmp == HAL_OK)
 	{
 		// Wait for the transmission to finish
 		xSemaphoreTake( _hRes->transmissionFinished, ( TickType_t ) portMAX_DELAY );
 	} else {
-		DEBUG("Failed SMBus transmission");
+		Debug::printf("Failed SMBus transmission!");
+		stateCode = HAL_SMBUS_GetState(&_hRes->handle);
+		errCode = HAL_SMBUS_GetError(&_hRes->handle);
+		Debug::printf("stateCode: %d     errCode: %d\n ",stateCode,errCode);
+	}
+
+	// Read data requested from register address
+	uint8_t readLength = 0;
+	XferOptions = SMBUS_NEXT_FRAME; //SMBUS_LAST_FRAME_WITH_PEC;//SMBUS_OTHER_FRAME_NO_PEC;
+	tmp = HAL_SMBUS_Master_Receive_IT(&_hRes->handle, (uint16_t)_devAddr, &readLength, 1, XferOptions);
+	if (tmp == HAL_OK)
+	{
+		// Wait for the the result
+		xSemaphoreTake( _hRes->transmissionFinished, ( TickType_t ) portMAX_DELAY );
+	} else {
+		Debug::printf("Failed SMBus reception!");
+		stateCode = HAL_SMBUS_GetState(&_hRes->handle);
+		errCode = HAL_SMBUS_GetError(&_hRes->handle);
+		Debug::printf("stateCode: %d     errCode: %d\n ",stateCode,errCode);
+	}
+
+	if (readLength > bufferLength){
+		ERROR("SMbus block read readLength > bufferLength");
+		return -1;
+	}
+
+	// Read data requested from register address
+	XferOptions = SMBUS_LAST_FRAME_NO_PEC; //SMBUS_LAST_FRAME_WITH_PEC;//SMBUS_OTHER_FRAME_NO_PEC;
+	tmp = HAL_SMBUS_Master_Receive_IT(&_hRes->handle, (uint16_t)_devAddr, buffer, readLength, XferOptions);
+	if (tmp == HAL_OK)
+	{
+		// Wait for the the result
+		xSemaphoreTake( _hRes->transmissionFinished, ( TickType_t ) portMAX_DELAY );
+	} else {
+		Debug::printf("Failed SMBus reception!");
+		stateCode = HAL_SMBUS_GetState(&_hRes->handle);
+		errCode = HAL_SMBUS_GetError(&_hRes->handle);
+		Debug::printf("stateCode: %d     errCode: %d\n ",stateCode,errCode);
 	}
 
-	//uint32_t errCode2 = HAL_SMBUS_GetError(&_hRes->handle);
 
 	xSemaphoreGive( _hRes->resourceSemaphore ); // give hardware resource back
+	return readLength;
 }
 
+
 void I2C2_EV_IRQHandler(void)
 {
 	if (SMBus::resI2C2)
@@ -369,13 +611,13 @@ void I2C2_EV_IRQHandler(void)
 void I2C2_ER_IRQHandler(void)
 {
 	if (SMBus::resI2C2)
-		HAL_SMBUS_ER_IRQHandler(&SMBus::resI2C4->handle);
+		HAL_SMBUS_ER_IRQHandler(&SMBus::resI2C2->handle);
 }
 
 void I2C4_EV_IRQHandler(void)
 {
-	if (SMBus::resI2C2)
-		HAL_SMBUS_EV_IRQHandler(&SMBus::resI2C2->handle);
+	if (SMBus::resI2C4)
+		HAL_SMBUS_EV_IRQHandler(&SMBus::resI2C4->handle);
 }
 void I2C4_ER_IRQHandler(void)
 {
diff --git a/KugleFirmware/Libraries/Periphirals/SMBus/SMBus.h b/KugleFirmware/Libraries/Periphirals/SMBus/SMBus.h
index 09c715d..223a6af 100644
--- a/KugleFirmware/Libraries/Periphirals/SMBus/SMBus.h
+++ b/KugleFirmware/Libraries/Periphirals/SMBus/SMBus.h
@@ -25,8 +25,8 @@
 class SMBus
 {
 	private:
-		const uint32_t SMBUS_DEFAULT_FREQUENCY = 100000;	// 100 kHz
-		const uint8_t SMBUS_MASTER_ADDRESS = 38;
+		const uint32_t SMBUS_DEFAULT_FREQUENCY = 50000;	// 100 kHz
+		const uint8_t SMBUS_MASTER_ADDRESS =  0x08; //38;
 
 	public:
 		typedef enum port_t {
@@ -35,6 +35,13 @@ class SMBus
 			PORT_I2C4
 		} port_t;
 
+/*		typedef enum DeviceAddr : uint8_t{ // according to smbus protocol v 1.1
+			host = 0x08,
+			Charger = 0x09,
+			Selector_SystemManager = 0x0A,
+			battery = 0x0B,
+		} smBusDeviceAddr;*/
+
 	public:
 		SMBus(port_t port, uint8_t devAddr); // use default frequency (or current configured frequency)
 		SMBus(port_t port, uint8_t devAddr, uint32_t frequency); // configure with frequency if possible
@@ -46,6 +53,8 @@ class SMBus
 		void Write(uint8_t reg, uint8_t value);
 		void Read(uint8_t reg, uint8_t * buffer, uint8_t readLength);
 		uint8_t Read(uint8_t reg);
+		int16_t blockRead(uint8_t reg, uint8_t * buffer, uint8_t bufferLength);
+		bool devicePresent();
 
 	public:
 		typedef struct hardware_resource_t {
diff --git a/KugleFirmware/Src/MainTask.cpp b/KugleFirmware/Src/MainTask.cpp
index 6eb9ec7..8079ba0 100644
--- a/KugleFirmware/Src/MainTask.cpp
+++ b/KugleFirmware/Src/MainTask.cpp
@@ -91,13 +91,6 @@ void MainTask(void * pvParameters)
 	/* Initialize global parameters */
 	Parameters& params = *(new Parameters);
 
-	/* Initialize power management */
-	IO * enable19V = new IO(GPIOE, GPIO_PIN_4); // configure as output
-	IO * enable5V = new IO(GPIOC, GPIO_PIN_3); // configure as output
-	Battery * battery1 = new Battery();
-	Battery * battery2 = new Battery();
-	PWM * powerLED = new PWM(PWM::TIMER17, PWM::CH1, POWER_LED_PWM_FREQUENCY, POWER_LED_PWM_RANGE);
-	PowerManagement * pm = new PowerManagement(*enable19V, *enable5V, *battery1, *battery2, *powerLED, POWER_MANAGEMENT_PRIORITY);
 
 	/* Initialize EEPROM */
 	EEPROM * eeprom = new EEPROM;
@@ -106,6 +99,9 @@ void MainTask(void * pvParameters)
 	eeprom->Initialize();
 	params.AttachEEPROM(eeprom); // attach EEPROM to load and store parameters into EEPROM
 
+	/* Initialize microseconds timer */
+	Timer * microsTimer = new Timer(Timer::TIMER6, 1000000); // create a 1 MHz counting timer used for micros() timing
+
 	/* Initialize MATLAB coder globals */
 	MATLABCoder_initialize();
 
@@ -119,6 +115,12 @@ void MainTask(void * pvParameters)
 	lspcUSB->registerCallback(lspc::MessageTypesFromPC::Reboot, &Reboot_Callback);
 	lspcUSB->registerCallback(lspc::MessageTypesFromPC::EnterBootloader, &EnterBootloader_Callback);
 
+	/* Initialize power management */
+	IO * enable19V = new IO(GPIOE, GPIO_PIN_4); // configure as output
+	IO * enable5V = new IO(GPIOC, GPIO_PIN_3); // configure as output
+	PWM * powerLED = new PWM(PWM::TIMER17, PWM::CH1, POWER_LED_PWM_FREQUENCY, POWER_LED_PWM_RANGE);
+	PowerManagement * pm = new PowerManagement(*enable19V, *enable5V, *powerLED, POWER_MANAGEMENT_PRIORITY, *lspcUSB, *microsTimer);
+
 	/* Debug boot info */
 	Debug::print("Booting...\n");
 
@@ -170,9 +172,6 @@ void MainTask(void * pvParameters)
 		imu->SetCalibration(params.sensor.default_accelerometer_bias, params.sensor.default_accelerometer_scale, params.sensor.default_gyroscope_bias, params.sensor.default_calibration_matrix);
 	}
 
-	/* Initialize microseconds timer */
-	Timer * microsTimer = new Timer(Timer::TIMER6, 1000000); // create a 1 MHz counting timer used for micros() timing
-
 	/* Initialize motors */
 	ESCON * motor1 = new ESCON(1, params.model.MotorMaxCurrent, params.model.MotorTorqueConstant, params.model.i_gear, params.model.EncoderTicksPrRev, params.model.MotorMaxSpeed);
 	ESCON * motor2 = new ESCON(2, params.model.MotorMaxCurrent, params.model.MotorTorqueConstant, params.model.i_gear, params.model.EncoderTicksPrRev, params.model.MotorMaxSpeed);
diff --git a/KugleFirmware/Src/TestBench.cpp b/KugleFirmware/Src/TestBench.cpp
index d4a651d..edfe1bd 100644
--- a/KugleFirmware/Src/TestBench.cpp
+++ b/KugleFirmware/Src/TestBench.cpp
@@ -20,6 +20,7 @@
 #include "cmsis_os.h"
 #include "FreeRTOS.h"
 #include "Priorities.h"
+#include "stm32h7xx_hal.h"
 
 #include <string>
 #include <stdlib.h>
@@ -41,6 +42,7 @@
 #include "FrontPanel.h"
 #include "VelocityEKF.h"
 
+
 void TestBench(void * pvParameters);
 TaskHandle_t testBenchTaskHandle;
 UART * uart;
@@ -71,6 +73,82 @@ float ax, ay, az, gx, gy, gz, mx, my, mz;
 
 float angle = 0;
 
+#if 1
+void TestBench(void * pvParameters)
+{
+
+		/* Initialize communication */
+		USBCDC * usb = new USBCDC(USBCDC_TRANSMITTER_PRIORITY);
+		LSPC * lspcUSB = new LSPC(usb, LSPC_RECEIVER_PRIORITY, LSPC_TRANSMITTER_PRIORITY); // very important to use "new", otherwise the object gets placed on the stack which does not have enough memory!
+		Debug * dbg = new Debug(lspcUSB); // pair debug module with configured LSPC module to enable "Debug::print" functionality
+		//params.AttachLSPC(lspcUSB); // attach USB object to allow modification of parameters over USB
+
+		/* Initialize microseconds timer */
+		Timer * microsTimer = new Timer(Timer::TIMER6, 1000000); // create a 1 MHz counting timer used for micros() timing
+
+		/* Initialize power management */
+		IO * enable19V = new IO(GPIOE, GPIO_PIN_4); // configure as output
+		IO * enable5V = new IO(GPIOC, GPIO_PIN_3); // configure as output
+		PWM * powerLED = new PWM(PWM::TIMER17, PWM::CH1, POWER_LED_PWM_FREQUENCY, POWER_LED_PWM_RANGE);
+		PowerManagement * pm = new PowerManagement(*enable19V, *enable5V, *powerLED, POWER_MANAGEMENT_PRIORITY, *lspcUSB, *microsTimer);
+		pm->SetPowerMode(pm->PowerMode_t::POWERMODE_ALL_ON);
+
+/*
+		SMBUS_HandleTypeDef Device;
+
+		const uint8_t SMBUS_MASTER_ADDRESS = 0x08; // smbus.h defined to 38
+
+		Device.Init.OwnAddress1 = SMBUS_MASTER_ADDRESS;
+		Device.Init.AnalogFilter = SMBUS_ANALOGFILTER_ENABLE;
+		Device.Init.AddressingMode = SMBUS_ADDRESSINGMODE_7BIT;
+		Device.Init.DualAddressMode = SMBUS_DUALADDRESS_DISABLE;
+		Device.Init.OwnAddress2 = 0;
+		Device.Init.OwnAddress2Masks = SMBUS_OA2_NOMASK;
+		Device.Init.GeneralCallMode = SMBUS_GENERALCALL_DISABLE;
+		Device.Init.NoStretchMode = SMBUS_NOSTRETCH_DISABLE;
+		Device.Init.PacketErrorCheckMode = SMBUS_PEC_ENABLE;//SMBUS_PEC_DISABLE;
+		Device.Init.PeripheralMode = SMBUS_PERIPHERAL_MODE_SMBUS_HOST;
+		Device.Init.SMBusTimeout = 0x000084C4;
+
+		uint32_t ErrCode = HAL_SMBUS_Init(&Device);
+		STACK_SMBUS_GetBuffer(&context);
+		STACK_SMBUS_HostCommand(&context,&cmd,addr,WRITE);
+*/
+
+		// poll stack context from when ready stat
+
+		/* Send CPU load every second */
+			char * pcWriteBuffer = (char *)pvPortMalloc(1024);
+			while (1)
+			{
+				vTaskGetRunTimeStats(pcWriteBuffer);
+				char * endPtr = &pcWriteBuffer[strlen(pcWriteBuffer)];
+				*endPtr++ = '\n'; *endPtr++ = '\n'; *endPtr++ = 0;
+
+				// Split into multiple packages and send
+				uint16_t txIdx = 0;
+				uint16_t remainingLength = strlen(pcWriteBuffer);
+				uint16_t txLength;
+
+				while (remainingLength > 0) {
+					txLength = remainingLength;
+					if (txLength > LSPC_MAXIMUM_PACKAGE_LENGTH) {
+						txLength = LSPC_MAXIMUM_PACKAGE_LENGTH;
+						while (pcWriteBuffer[txIdx+txLength] != '\n' && txLength > 0) txLength--; // find and include line-break (if possible)
+						if (txLength == 0) txLength = LSPC_MAXIMUM_PACKAGE_LENGTH;
+						else txLength++;
+					}
+					lspcUSB->TransmitAsync(lspc::MessageTypesToPC::CPUload, (uint8_t *)&pcWriteBuffer[txIdx], txLength);
+
+					txIdx += txLength;
+					remainingLength -= txLength;
+				}
+				osDelay(1000);
+			}
+}
+#endif
+
+
 #if 0
 void TestBench(void * pvParameters)
 {
diff --git a/KugleFirmware/Src/main.c b/KugleFirmware/Src/main.c
index d76ffeb..12d2f35 100644
--- a/KugleFirmware/Src/main.c
+++ b/KugleFirmware/Src/main.c
@@ -58,7 +58,7 @@ int main(void)
   ZeroInitFreeRTOSheap();
 
   /* Create the main thread which creates objects and spawns the rest of the threads */
-  xTaskCreate(MainTask, "mainTask", 1024, (void*) NULL, MAIN_TASK_PRIORITY, &mainTaskHandle);
+  xTaskCreate(MainTask, "mainTask", 1024, (void*) NULL, MAIN_TASK_PRIORITY, &mainTaskHandle); // udkommenter igen
 
   /* Start scheduler */
   osKernelStart();