WIP

CMakeLists.txt

@@ -2,6 +2,8 @@ cmake_minimum_required(VERSION 3.16)
 
 project(qtrocket VERSION 0.1 LANGUAGES CXX)
 
+set(CMAKE_CXX_STANDARD 20)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
 
 include(FetchContent)
 
@@ -42,13 +44,13 @@ FetchContent_Declare(eigen
    GIT_TAG 3.4.0)
 FetchContent_MakeAvailable(eigen)
 
+# Add qtrocket subdirectories. These are components that will be linked in 
+
 
 set(CMAKE_AUTOUIC ON)
 set(CMAKE_AUTOMOC ON)
 set(CMAKE_AUTORCC ON)
 
-set(CMAKE_CXX_STANDARD 20)
-set(CMAKE_CXX_STANDARD_REQUIRED ON)
 
 if(WIN32)
   set(CMAKE_PREFIX_PATH $ENV{QTDIR})
@@ -112,6 +114,7 @@ set(PROJECT_SOURCES
         sim/SphericalGravityModel.cpp
         sim/SphericalGravityModel.h
         sim/StateData.h
+        sim/TestRK4Solver.h
         sim/USStandardAtmosphere.cpp
         sim/USStandardAtmosphere.h
         sim/WindModel.cpp
@@ -162,10 +165,8 @@ else()
         )
     endif()
 
-    #qt5_create_translation(QM_FILES ${CMAKE_SOURCE_DIR} ${TS_FILES})
 endif()
 
-#target_link_libraries(qtrocket PRIVATE Qt${QT_VERSION_MAJOR}::Widgets Qt${Qt_VERSION_MAJOR}::PrintSupport libcurl jsoncpp_static fmt::fmt-header-only)
 target_link_libraries(qtrocket PRIVATE Qt6::Widgets Qt6::PrintSupport libcurl jsoncpp_static fmt::fmt-header-only eigen)
 
 set_target_properties(qtrocket PROPERTIES

QtRocket.h

@@ -59,7 +59,7 @@ public:
     * @brief getStates returns a vector of time/state pairs generated during launch()
     * @return vector of pairs of doubles, where the first value is a time and the second a state vector
     */
-   const std::vector<std::pair<double, std::vector<double>>>& getStates() const { return rocket.second->getStates(); }
+   const std::vector<std::pair<double, Vector6>>& getStates() const { return rocket.second->getStates(); }
 
    /**
     * @brief setInitialState sets the initial state of the Rocket.

gui/AnalysisWindow.cpp

@@ -36,7 +36,7 @@ AnalysisWindow::~AnalysisWindow()
 void AnalysisWindow::onButton_plotAltitude_clicked()
 {
    QtRocket* qtRocket = QtRocket::getInstance();
-   const std::vector<std::pair<double, std::vector<double>>>& res = qtRocket->getStates();
+   const std::vector<std::pair<double, Vector6>>& res = qtRocket->getStates();
    auto& plot = ui->plotWidget;
    plot->clearGraphs();
    plot->setInteraction(QCP::iRangeDrag, true);
@@ -63,7 +63,7 @@ void AnalysisWindow::onButton_plotAltitude_clicked()
 void AnalysisWindow::onButton_plotVelocity_clicked()
 {
    QtRocket* qtRocket = QtRocket::getInstance();
-   const std::vector<std::pair<double, std::vector<double>>>& res = qtRocket->getStates();
+   const std::vector<std::pair<double, Vector6>>& res = qtRocket->getStates();
    auto& plot = ui->plotWidget;
    plot->clearGraphs();
    plot->setInteraction(QCP::iRangeDrag, true);

main.cpp

@@ -23,45 +23,4 @@ int main(int argc, char *argv[])
    int retVal = qtrocket->run(argc, argv);
    logger->debug("Returning");
    return retVal;
-}
+}
-
-
-
-/*
- * This was the old main when it directly started the QtGui. It worked. The new way of
- * starting the gui through QtRocket->run() also seems to work, but I'm leaving this here for
- * now in case there are unforeseen issues with starting the gui in a separate thread via
- * QtRocket::run()
- *
-
-#include "gui/MainWindow.h"
-
-#include <QApplication>
-#include <QLocale>
-#include <QTranslator>
-
-int main(int argc, char* argv[])
-{
-   QApplication a(argc, argv);
-   a.setWindowIcon(QIcon(":/qtrocket.png"));
-
-   // Start translation component.
-   // TODO: Only support US English at the moment. Anyone want to help translate?
-   QTranslator translator;
-   const QStringList uiLanguages = QLocale::system().uiLanguages();
-   for (const QString &locale : uiLanguages)
-   {
-      const QString baseName = "qtrocket_" + QLocale(locale).name();
-      if (translator.load(":/i18n/" + baseName))
-      {
-         a.installTranslator(&translator);
-         break;
-      }
-   }
-
-   // Go!
-   MainWindow w;
-   w.show();
-   return a.exec();
-}
-*/

model/Rocket.cpp

@@ -1,3 +1,5 @@
+
+// qtrocket headers
 #include "Rocket.h"
 #include "QtRocket.h"
 
@@ -16,7 +18,7 @@ void Rocket::setMotorModel(const model::MotorModel& motor)
    mm = motor;
 }
 
-bool Rocket::terminateCondition(const std::pair<double, std::vector<double>>& cond)
+bool Rocket::terminateCondition(const std::pair<double, Vector6>& cond)
 {
    // Terminate propagation when the z coordinate drops below zero
     if(cond.second[2] < 0)

model/Rocket.h

@@ -15,6 +15,7 @@
 #include "model/ThrustCurve.h"
 #include "model/MotorModel.h"
 #include "sim/Propagator.h"
+#include "utils/math/MathTypes.h"
 
 /**
  * @brief The Rocket class holds all rocket components
@@ -86,7 +87,7 @@ public:
     * @param cond time/state pair
     * @return true if the passed-in time/state satisfies the terminate condition
     */
-   bool terminateCondition(const std::pair<double, std::vector<double>>& cond);
+   bool terminateCondition(const std::pair<double, Vector6>& cond);
 
    /**
     * @brief setName sets the rocket name
@@ -98,8 +99,6 @@ public:
 
 private:
 
-
-   std::vector<double> bodyFrameState{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
    std::string name; /// Rocket name
    double dragCoeff; /// @todo get rid of this, should be dynamically calculated
    double mass; /// @todo get rid of this, should be dynamically computed, but is the current rocket mass

sim/DESolver.h

@@ -9,9 +9,13 @@
 // 3rd party headers
 /// \endcond
 
+// qtrocket headers
+#include "utils/math/MathTypes.h"
+
 namespace sim
 {
 
+template<typename T>
 class DESolver
 {
 public:
@@ -29,7 +33,7 @@ public:
     *          the RK4 solver independently as a general tool, this interface is needed
     *          here unfortunately.
     */
-   virtual void step(const std::vector<double>& curVal, std::vector<double>& res, double t = 0.0) = 0;
+   virtual std::pair<T, T> step(T& state, T& rate) = 0;
 };
 
 } // namespace sim

sim/GravityModel.h

@@ -14,7 +14,7 @@ public:
    virtual ~GravityModel() {}
 
    virtual Vector3 getAccel(double x, double y, double z) = 0;
-   Vector3 getAccel(const Vector3& t) { return this->getAccel(t[0], t[1], t[2]); }
+   Vector3 getAccel(const Vector3& t) { return this->getAccel(t.x(), t.y(), t.z()); }
 };
 
 } // namespace sim

sim/Propagator.cpp

@@ -24,9 +24,10 @@ namespace sim {
 
 Propagator::Propagator(std::shared_ptr<Rocket> r)
    : linearIntegrator(),
-     //orientationIntegrator(),
+     orientationIntegrator(),
-     rocket(r)
+     rocket(r),
-
+     currentBodyState(),
+     worldFrameState()
 {
 
 
@@ -38,23 +39,78 @@ Propagator::Propagator(std::shared_ptr<Rocket> r)
    // and pass it a freshly allocated RK4Solver pointer
 
    // The state vector has components of the form:
-   linearIntegrator.reset(new RK4Solver(
+
-      /* dx/dt  */ [](const std::vector<double>& s, double) -> double {return s[3]; },
+   // Linear velocity and acceleration
-      /* dy/dt  */ [](const std::vector<double>& s, double) -> double {return s[4]; },
+   std::function<std::pair<Vector3, Vector3>(Vector3&, Vector3&)> linearODE = [this](Vector3& state, Vector3& rate) -> std::pair<Vector3, Vector3>
-      /* dz/dt  */ [](const std::vector<double>& s, double) -> double {return s[5]; },
+   {
-      /* dvx/dt */ [this](const std::vector<double>&, double ) -> double { return getForceX() / getMass(); },
+      Vector3 dPosition;
-      /* dvy/dt */ [this](const std::vector<double>&, double ) -> double { return getForceY() / getMass(); },
+      Vector3 dVelocity;
-      /* dvz/dt */ [this](const std::vector<double>&, double ) -> double { return getForceZ() / getMass(); }));
+      // dx/dt
+      dPosition[0] = rate[0];
+
+      // dy/dt
+      dPosition[1] = rate[1];
+
+      // dz/dt
+      dPosition[2] = rate[2];
+
+      // dvx/dt
+      dVelocity[0] = getForceX() / getMass();
+
+      // dvy/dt
+      dVelocity[1] = getForceY() / getMass();
+
+      // dvz/dt
+      dVelocity[2] = getForceZ() / getMass();
+
+      return std::make_pair(dPosition, dVelocity);
+
+   };
+
+   linearIntegrator.reset(new RK4Solver<Vector6>(linearODE));
    linearIntegrator->setTimeStep(timeStep);
    
+   // This is pure quaternion
+   // This formula is taken from https://www.vectornav.com/resources/inertial-navigation-primer/math-fundamentals/math-attitudetran
+   //
+   // Convention for the quaternions is (vector, scalar). So q4 is scalar term
+   //
+   // q1Dot = 1/2 * [(q4*omega1) - (q3*omega2) + (q2*omega3)]
+   // q2Dot = 1/2 * [(q3*omega1) + (q4*omega2) - (q1*omega3)]
+   // q3Dot = 1/2 * [(-q2*omega1) + (q1*omega2) + (q4*omega3)]
+   // q4Dot = -1/2 *[(q1*omega1) + (q2*omega2) + (q3*omega3)]
+   //
+   // omega1 = yawRate
+   // omega2 = pitchRate
+   // omega3 = rollRate
+   //
 //   orientationIntegrator.reset(new RK4Solver(
-//       /* dpitch/dt    */ [](double, const std::vector<double>& s) -> double { return s[3]; },
+//       /* dyawRate/dt    */ [this](const std::vector<double>& s, double) -> double
-//       /* dyaw/dt      */ [](double, const std::vector<double>& s) -> double { return s[4]; },
+//       { return getTorqueP() / getIyaw(); },
-//       /* droll/dt     */ [](double, const std::vector<double>& s) -> double { return s[5]; },
+//       /* dpitchRate/dt  */ [this](const std::vector<double>& s, double) -> double
-//       /* dpitchRate/dt */ [this](double, const std::vector<double>& s) -> double { return (getTorqueP() - s[1] * s[2] * (getIroll() - getIyaw())) / getIpitch(); },
+//       { return getTorqueQ() / getIpitch(); },
-//       /* dyawRate/dt   */ [this](double, const std::vector<double>& s) -> double { return (getTorqueQ() - s[0] * s[2] * (getIpitch() - getIroll())) / getIyaw(); },
+//       /* drollRate/dt   */ [this](const std::vector<double>& s, double) -> double
-//      /* drollRate/dt   */ [this](double, const std::vector<double>& s) -> double { return (getTorqueR() - s[0] * s[1] * (getIyaw() - getIpitch())) / getIroll(); }));
+//       { return getTorqueR() / getIroll(); },
-
+//       /* q1Dot */ [](const std::vector<double>& s, double) -> double
+//       {
+//           double retVal = s[6]*s[0] - s[5]*s[1] + s[4]*s[2];
+//           return 0.5 * retVal;
+//       },
+//       /* q2Dot */ [](const std::vector<double>& s, double) -> double
+//       {
+//           double retVal = s[5]*s[0] + s[6]*s[1] - s[3]*s[2];
+//           return 0.5 * retVal;
+//       },
+//       /* q3Dot */ [](const std::vector<double>& s, double) -> double
+//       {
+//           double retVal = -s[4]*s[0] + s[3]*s[1] + s[6]*s[2];
+//           return 0.5 * retVal;
+//       },
+//       /* q4Dot */ [](const std::vector<double>& s, double) -> double
+//       {
+//           double retVal = s[3]*s[0] + s[4]*s[1] + s[5]*s[2];
+//           return -0.5 * retVal;
+//       }));
 //   orientationIntegrator->setTimeStep(timeStep);
 
    saveStates = true;
@@ -71,15 +127,22 @@ void Propagator::runUntilTerminate()
 
    while(true)
    {
-      // tempRes gets overwritten
-      linearIntegrator->step(currentState, tempRes);
 
-      std::swap(currentState, tempRes);
+      // Reset the body frame positions to zero since the origin of the body frame is the CM
+      currentBodyState[0] = 0.0;
+      currentBodyState[1] = 0.0;
+      currentBodyState[2] = 0.0;
+
+      // tempRes gets overwritten
+      tempRes = linearIntegrator->step(currentBodyState);
+
+      std::swap(currentBodyState, tempRes);
+      
       if(saveStates)
       {
-         states.push_back(std::make_pair(currentTime, currentState));
+         states.push_back(std::make_pair(currentTime, currentBodyState));
       }
-      if(rocket->terminateCondition(std::make_pair(currentTime, currentState)))
+      if(rocket->terminateCondition(std::make_pair(currentTime, currentBodyState)))
          break;
 
       currentTime += timeStep;
@@ -101,20 +164,20 @@ double Propagator::getMass()
 double Propagator::getForceX()
 {
     QtRocket* qtrocket = QtRocket::getInstance();
-    return (currentState[3] >= 0 ? -1.0 : 1.0) *  qtrocket->getEnvironment()->getAtmosphericModel()->getDensity(currentState[2])/ 2.0 * 0.008107 * rocket->getDragCoefficient() * currentState[3]* currentState[3];
+    return (currentBodyState[3] >= 0 ? -1.0 : 1.0) *  qtrocket->getEnvironment()->getAtmosphericModel()->getDensity(currentBodyState[2])/ 2.0 * 0.008107 * rocket->getDragCoefficient() * currentBodyState[3]* currentBodyState[3];
 }
 
 double Propagator::getForceY()
 {
     QtRocket* qtrocket = QtRocket::getInstance();
-    return (currentState[4] >= 0 ? -1.0 : 1.0) * qtrocket->getEnvironment()->getAtmosphericModel()->getDensity(currentState[2]) / 2.0 * 0.008107 * rocket->getDragCoefficient() * currentState[4]* currentState[4];
+    return (currentBodyState[4] >= 0 ? -1.0 : 1.0) * qtrocket->getEnvironment()->getAtmosphericModel()->getDensity(currentBodyState[2]) / 2.0 * 0.008107 * rocket->getDragCoefficient() * currentBodyState[4]* currentBodyState[4];
 }
 
 double Propagator::getForceZ()
 {
     QtRocket* qtrocket = QtRocket::getInstance();
-    double gravity = (qtrocket->getEnvironment()->getGravityModel()->getAccel(currentState[0], currentState[1], currentState[2]))[2];
+    double gravity = (qtrocket->getEnvironment()->getGravityModel()->getAccel(currentBodyState[0], currentBodyState[1], currentBodyState[2]))[2];
-    double airDrag = (currentState[5] >= 0 ? -1.0 : 1.0) * qtrocket->getEnvironment()->getAtmosphericModel()->getDensity(currentState[2]) / 2.0 * 0.008107 * rocket->getDragCoefficient() * currentState[5]* currentState[5];
+    double airDrag = (currentBodyState[5] >= 0 ? -1.0 : 1.0) * qtrocket->getEnvironment()->getAtmosphericModel()->getDensity(currentBodyState[2]) / 2.0 * 0.008107 * rocket->getDragCoefficient() * currentBodyState[5]* currentBodyState[5];
     double thrust  = rocket->getThrust(currentTime);
     return gravity + airDrag + thrust;
 }
@@ -123,4 +186,28 @@ double Propagator::getTorqueP() { return 0.0; }
 double Propagator::getTorqueQ() { return 0.0; }
 double Propagator::getTorqueR() { return 0.0; }
 
+Vector3 Propagator::getCurrentGravity()
+{
+   auto gravityModel = QtRocket::getInstance()->getEnvironment()->getGravityModel();
+   auto gravityAccel = gravityModel->getAccel(currentBodyState[0],
+                                              currentBodyState[1],
+                                              currentBodyState[2]);
+   Vector3 gravityVector{gravityAccel[0],
+                         gravityAccel[1],
+                         gravityAccel[2]};
+   
+
+   Quaternion q{currentOrientation[3],
+                currentOrientation[4],
+                currentOrientation[5],
+                currentOrientation[6]};
+
+   Vector3 res = q * gravityVector;
+   
+
+   return Vector3{0.0, 0.0, 0.0};
+
+
+}
+
 } // namespace sim

sim/Propagator.h

@@ -12,6 +12,8 @@
 
 // qtrocket headers
 #include "sim/DESolver.h"
+#include "utils/math/MathTypes.h"
+#include "sim/StateData.h"
 
 
 // Forward declare
@@ -29,17 +31,16 @@ public:
 
     void setInitialState(const std::vector<double>& initialState)
     {
-       currentState.resize(initialState.size());
        for(std::size_t i = 0; i < initialState.size(); ++i)
        {
-           currentState[i] = initialState[i];
+           currentBodyState[i] = initialState[i];
        }
 
     }
 
-    const std::vector<double>& getCurrentState() const
+    const Vector6& getCurrentState() const
     {
-       return currentState;
+       return currentBodyState;
     }
 
     void runUntilTerminate();
@@ -49,43 +50,56 @@ public:
        saveStates = s;
     }
 
-    const std::vector<std::pair<double, std::vector<double>>>& getStates() const { return states; }
+    const std::vector<std::pair<double, Vector6>>& getStates() const { return states; }
 
     void clearStates() { states.clear(); }
     void setCurrentTime(double t) { currentTime = t; }
-
     void setTimeStep(double ts) { timeStep = ts; }
-
     void setSaveStats(bool s) { saveStates = s; }
 
 private:
-    double getMass();
+   double getMass();
-    double getForceX();
+   double getForceX();
-    double getForceY();
+   double getForceY();
-    double getForceZ();
+   double getForceZ();
 
-    double getTorqueP();
+   double getTorqueP();
-    double getTorqueQ();
+   double getTorqueQ();
-    double getTorqueR();
+   double getTorqueR();
 
-    double getIpitch() { return 1.0; }
+   double getIpitch() { return 1.0; }
-    double getIyaw()   { return 1.0; }
+   double getIyaw()   { return 1.0; }
-    double getIroll()  { return 1.0; }
+   double getIroll()  { return 1.0; }
 
-//private:
+   std::unique_ptr<sim::RK4Solver<Vector3>> linearIntegrator;
-
+   std::unique_ptr<sim::RK4Solver<Quaternion>> orientationIntegrator;
-   std::unique_ptr<sim::DESolver> linearIntegrator;
-   //std::unique_ptr<sim::DESolver> orientationIntegrator;
 
    std::shared_ptr<Rocket> rocket;
 
+   StateData worldFrameState;
+   //StateData bodyFrameState;
+   Vector3 currentBodyPosition{0.0, 0.0, 0.0};
+   Vector3 currentBodyVelocity{0.0, 0.0, 0.0};
+   Vector3 nextBodyPosition{0.0, 0.0, 0.0};
+   Vector3 nextBodyVelocity{0.0, 0.0, 0.0};
 
-   std::vector<double> currentState{0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
+   std::vector<double> currentWorldState{0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
-   std::vector<double> tempRes{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
+
+   Vector3 currentGravity{0.0, 0.0, 0.0};
+   Vector3 currentWindSpeed{0.0, 0.0, 0.0};
+   
+
+   // orientation vectors in the form (yawDot, pitchDot, rollDot, q1, q2, q3, q4)
+   Quaternion currentOrientation{0.0, 0.0, 0.0, 0.0};
+   Quaternion currentOrientationRate{0.0, 0.0, 0.0, 0.0};
+   Quaternion nextOrientation{0.0, 0.0, 0.0, 0.0};
+   Quaternion nextOrientationRate{0.0, 0.0, 0.0, 0.0};
    bool saveStates{true};
    double currentTime{0.0};
    double timeStep{0.01};
-   std::vector<std::pair<double, std::vector<double>>> states;
+   std::vector<std::pair<double, Vector6>> states;
+
+   Vector3 getCurrentGravity();
 };
 
 } // namespace sim

sim/RK4Solver.h

@@ -16,6 +16,7 @@
 // qtrocket headers
 #include "sim/DESolver.h"
 #include "utils/Logger.h"
+#include "utils/math/MathTypes.h"
 
 namespace sim {
 
@@ -28,81 +29,67 @@ namespace sim {
  * 
  * @tparam Ts 
  */
-template<typename... Ts>
+template<typename T>
-class RK4Solver : public DESolver
+class RK4Solver : public DESolver<T>
 {
 public:
 
-   RK4Solver(Ts... funcs)
+   RK4Solver(std::function<std::pair<T, T>(T&, T&)> func)
    {
-      (odes.push_back(funcs), ...);
+      // This only works for Eigen Vector types.
-      temp.resize(sizeof...(Ts));
+      // TODO: Figure out how to make this slightly more generic, but for now
-
+      // we're only using this for Vector3 and Quaternion types
+      static_assert(std::is_same<T, Vector3>::value
+                    || std::is_same<T, Quaternion>::value,
+                    "You can only use Vector3 or Quaternion valued functions in RK4Solver");
+      
+      odes = func;
    }
    virtual ~RK4Solver() {}
 
    void setTimeStep(double inTs) override { dt = inTs;  halfDT = dt / 2.0; }
 
-   void step(const std::vector<double>& curVal, std::vector<double>& res, double t = 0.0) override
+   std::pair<T, T> step(T& state, T& rate) override
    {
+      std::pair<T, T> res;
       if(dt == std::numeric_limits<double>::quiet_NaN())
       {
          utils::Logger::getInstance()->error("Calling RK4Solver without setting dt first is an error");
-         res[0] = std::numeric_limits<double>::quiet_NaN();
+         return res;
       }
 
-      for(size_t i = 0; i < len; ++i)
+      std::tie(k1State, k1Rate) = odes(state, rate);
-      {
-         k1[i] = odes[i](curVal, t);
-      }
       // compute k2 values. This involves stepping the current values forward a half-step
       // based on k1, so we do the stepping first
-      for(size_t i = 0; i < len; ++i)
+      std::tie(tempState, tempRate) = std::make_pair(state + k1State*halfDT, rate + k1Rate*halfDT);
-      {
+      std::tie(k2State, k2Rate) = odes(tempState, tempRate);
-         temp[i] = curVal[i] + k1[i]*dt / 2.0;
-      }
-      for(size_t i = 0; i < len; ++i)
-      {
-         k2[i] = odes[i](temp, t + halfDT);
-      }
-      // repeat for k3
-      for(size_t i = 0; i < len; ++i)
-      {
-         temp[i] = curVal[i] + k2[i]*dt / 2.0;
-      }
-      for(size_t i = 0; i < len; ++i)
-      {
-         k3[i] = odes[i](temp, t + halfDT);
-      }
 
-      // now k4
+      std::tie(tempState, tempRate) = std::make_pair(state + k2State*halfDT, rate + k2Rate*halfDT);
-      for(size_t i = 0; i < len; ++i)
+      std::tie(k3State, k3Rate) = odes(tempState, tempRate);
-      {
-         temp[i] = curVal[i] + k3[i]*dt;
-      }
-      for(size_t i = 0; i < len; ++i)
-      {
-         k4[i] = odes[i](temp, t + dt);
-      }
 
-      // now compute the result
+      std::tie(tempState, tempRate) = std::make_pair(state + k3State*dt, rate + k3Rate*dt);
-      for(size_t i = 0; i < len; ++i)
+      std::tie(k4State, k4Rate) = odes(tempState, tempRate);
-      {
-         res[i] = curVal[i] + (dt / 6.0)*(k1[i] + 2.0*k2[i] + 2.0*k3[i] + k4[i]);
-      }
 
+      res = std::make_pair(state + (dt / 6.0)*(k1State + 2.0*k2State + 2.0*k3State + k4State),
+                           rate  + (dt / 6.0)*(k1Rate +  2.0*k2Rate  + 2.0*k3Rate  + k4Rate));
+
+      return res;
    }
 
 private:
-   std::vector<std::function<double(const std::vector<double>&, double)>> odes;
+   std::function<std::pair<T, T>(T&, T&)> odes;
 
-   static constexpr size_t len = sizeof...(Ts);
+   T k1State;
-   double k1[len];
+   T k2State;
-   double k2[len];
+   T k3State;
-   double k3[len];
+   T k4State;
-   double k4[len];
+   T k1Rate;
+   T k2Rate;
+   T k3Rate;
+   T k4Rate;
 
-   std::vector<double> temp;
+   T tempState;
+   T tempRate;
 
    double dt = std::numeric_limits<double>::quiet_NaN();
    double halfDT = 0.0;

sim/StateData.h

@@ -1,6 +1,13 @@
 #ifndef STATEDATA_H
 #define STATEDATA_H
 
+/// \cond
+// C headers
+// C++ headers
+#include <vector>
+// 3rd party headers
+/// \endcond
+
 // qtrocket headers
 #include "utils/math/MathTypes.h"
 
@@ -15,17 +22,33 @@ public:
    StateData() {}
    ~StateData() {}
 
+   std::vector<double> getPosStdVector()
+   {
+      return std::vector<double>{position[0], position[1], position[2]};
+   }
+   std::vector<double> getVelStdVector()
+   {
+      return std::vector<double>{velocity[0], velocity[1], velocity[2]};
+   }
+
 /// TODO: Put these behind an interface
 // private:
+
+   // These are 4-vectors so quaternion multiplication works out. The last term (scalar) is always
+   // zero. I could just use quaternions here, but I want to make it clear that these aren't
+   // rotations, they're vectors
    Vector3 position{0.0, 0.0, 0.0};
    Vector3 velocity{0.0, 0.0, 0.0};
 
-   Quaternion orientation{0.0, 0.0, 0.0, 0.0}; /// (scalar, vector)
+   Quaternion orientation{0.0, 0.0, 0.0, 0.0}; /// (vector, scalar)
-   Quaternion orientationRate{0.0, 0.0, 0.0, 0.0}; /// (scalar, vector)
+   Quaternion orientationRate{0.0, 0.0, 0.0, 0.0}; /// (vector, scalar)
 
    Matrix3 dcm{{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
 
    /// Euler angles are yaw-pitch-roll, and (3-2-1) order
+   /// yaw   - psi
+   /// pitch - theta
+   /// roll  - phi
    Vector3 eulerAngles{0.0, 0.0, 0.0};
 
    // This is an array because the integrator expects it

utils/math/MathTypes.h

@@ -6,10 +6,18 @@
 /// This is not in any namespace. These typedefs are intended to be used throughout QtRocket,
 /// so keeping them in the global namespace seems to make sense.
 
-typedef Eigen::Matrix3d    Matrix3;
+template <int Size>
-typedef Eigen::Matrix4d    Matrix4;
+using MyMatrix = Eigen::Matrix<double, Size, Size>;
-typedef Eigen::Vector3d    Vector3;
+
+template <int Size>
+using MyVector = Eigen::Matrix<double, Size, 1>;
+
 typedef Eigen::Quaterniond Quaternion;
 
+using Matrix3 = MyMatrix<3>;
+using Matrix4 = MyMatrix<4>;
+
+using Vector3 = MyVector<3>;
+using Vector6 = MyVector<6>;
 
 #endif // UTILS_MATH_MATHTYPES_H