Update NextStateSet method in TwoTrackMotionModel

sim/src/components/Dynamics_MotionModel/src/motionmodel.cpp

@@ -368,9 +368,10 @@ void Dynamics_MotionModel_Implementation::NextStateSet()
   velocityCar.Rotate(vehicle->GetYawAngle());  // global CS Reference COG
 
   // Transformation COG -> middle rear Axle
-  GetAgent()->SetVelocityVector({velocityCar.x - vehicle->GetYawVelocity() * (-vehicle->GetPositionCOG().y),
-                                 velocityCar.y + vehicle->GetYawVelocity() * (-vehicle->GetPositionCOG().x),
-                                 0.0_mps});
+  GetAgent()->SetVelocityVector(
+      {velocityCar.x - vehicle->GetYawVelocity().value() * (-vehicle->GetPositionCOG().y.value() * 1_mps),
+       velocityCar.y + vehicle->GetYawVelocity().value() * (-vehicle->GetPositionCOG().x.value() * 1_mps),
+       0.0_mps});
   GetAgent()->SetYawRate(vehicle->GetYawVelocity());
   velocityCar.Rotate(-vehicle->GetYawAngle());  // vehicle CS
 
@@ -383,10 +384,14 @@ void Dynamics_MotionModel_Implementation::NextStateSet()
   Common::Vector2d acceleration = vehicle->GetAcceleration();
   acceleration.Rotate(vehicle->GetYawAngle());
   GetAgent()->SetAccelerationVector(
-      {acceleration.x + (-vehicle->GetYawAcceleration() * (-vehicle->GetPositionCOG().y))
-           + (-vehicle->GetYawVelocity() * vehicle->GetYawVelocity() * (-vehicle->GetPositionCOG().x)),
-       acceleration.y + (vehicle->GetYawAcceleration() * (-vehicle->GetPositionCOG().x))
-           + (-vehicle->GetYawVelocity() * vehicle->GetYawVelocity() * (-vehicle->GetPositionCOG().y)),
+      {acceleration.x + 1_mps_sq * (-vehicle->GetYawAcceleration().value() * (-vehicle->GetPositionCOG().y.value()))
+           + 1_mps_sq
+                 * (-vehicle->GetYawVelocity().value() * vehicle->GetYawVelocity().value()
+                    * (-vehicle->GetPositionCOG().x.value())),
+       acceleration.y + 1_mps_sq * (vehicle->GetYawAcceleration().value() * (-vehicle->GetPositionCOG().x.value()))
+           + 1_mps_sq
+                 * (-vehicle->GetYawVelocity().value() * vehicle->GetYawVelocity().value()
+                    * (-vehicle->GetPositionCOG().y.value())),
        0.0_mps_sq});
 
   // update outputs

sim/src/components/Dynamics_MotionModel/src/twotrackmodel.cpp

@@ -96,7 +96,8 @@ void TwoTrackVehicleModel::InitVehicleProperties(std::vector<mantle_api::Axle> a
 
 void TwoTrackVehicleModel::ForceGlobal()
 {
-  forceTotalXY.Scale(0.0_N);
+  forceTotalXY.x = 0.0_N;
+  forceTotalXY.y = 0.0_N;
   momentTotalZ = 0.0_Nm;
   for (int i = 0; i < tireForce.size(); ++i)
   {