├── .gitignore ├── Codegen ├── CodegenExample.m ├── CodegenToMEX.m └── CoderConfig_ARM_CortexM.mat ├── Controllers ├── BalanceLQR │ ├── BalanceLQR.m │ ├── GainComputation.m │ ├── GainComputation_UsingLinearizedMatrices.m │ └── generated │ │ └── BalanceLQRgain.mat ├── MPC │ ├── CompareMPCmodels.m │ ├── EvaluateLinearMPCmodel.m │ ├── EvaluateNonlinearMPCmodel.m │ ├── ExtractDistanceTrajectory_mex.mexw64 │ ├── GenerateMPC.m │ ├── GenerateOptimizedMEX.m │ ├── SimulateMPC.m │ ├── TestConstrainedLeastSquared.m │ ├── TestGeneratedMPC.m │ ├── TestPolynomialFitting.m │ ├── TestPolynomialFunctions.m │ ├── functions │ │ ├── ConstrainedLeastSquares.m │ │ ├── DefineWeightedStates.m │ │ ├── DefineWeightedTerminalStates.m │ │ ├── WriteArrayIndexFile.m │ │ ├── polynomial │ │ │ ├── ArcLengthApproximation.m │ │ │ ├── ArcLengthApproximationNumerical.m │ │ │ ├── ComputeDerivativePolynomialCoefficients.m │ │ │ ├── ComputeSquaredPolynomialCoefficients.m │ │ │ ├── ConstrainedPolyFit.m │ │ │ ├── EvaluatePolynomial.m │ │ │ ├── EvaluatePolynomialSymbolic.m │ │ │ ├── PolynomialMinimumFinder.m │ │ │ ├── PolynomialMinimumFinderNumerical.m │ │ │ ├── PolynomialMinimumFinderNumerical2.m │ │ │ └── PolynomialMinimumFinderWithBounds.m │ │ ├── trajectory │ │ │ ├── ExtractDistanceTrajectory.m │ │ │ ├── ExtractWindowTrajectory.m │ │ │ ├── FitReferencePathPolynomial.m │ │ │ ├── GenerateTestTrajectory.m │ │ │ ├── GenerateTestTrajectory_FigureEight.m │ │ │ ├── GetNearestObstacles.m │ │ │ └── RotateTrajectory.m │ │ └── visualization │ │ │ ├── PlotAxRobotWithTiltAndVelocity.m │ │ │ ├── PlotCircle.m │ │ │ ├── PlotRobot.m │ │ │ ├── PlotRobotWithTiltAndVelocity.m │ │ │ ├── PlotRobotWithVelocity.m │ │ │ └── TrajectoryVisualizer.m │ └── generated │ │ ├── ExtractDistanceTrajectory_mex.mexw64 │ │ ├── ExtractWindowTrajectory_mex.mexw64 │ │ ├── FitReferencePathPolynomial_mex.mexw64 │ │ ├── codegen │ │ └── mex │ │ │ ├── ExtractDistanceTrajectory │ │ │ ├── ExtractDistanceTrajectory.c │ │ │ ├── ExtractDistanceTrajectory.h │ │ │ ├── ExtractDistanceTrajectory_data.c │ │ │ ├── ExtractDistanceTrajectory_data.h │ │ │ ├── ExtractDistanceTrajectory_emxutil.c │ │ │ ├── ExtractDistanceTrajectory_emxutil.h │ │ │ ├── ExtractDistanceTrajectory_initialize.c │ │ │ ├── ExtractDistanceTrajectory_initialize.h │ │ │ ├── ExtractDistanceTrajectory_mex.bat │ │ │ ├── ExtractDistanceTrajectory_mex.exp │ │ │ ├── ExtractDistanceTrajectory_mex.mexw64 │ │ │ ├── ExtractDistanceTrajectory_mex.mexw64.manifest │ │ │ ├── ExtractDistanceTrajectory_mex.mk │ │ │ ├── ExtractDistanceTrajectory_mex.mki │ │ │ ├── ExtractDistanceTrajectory_mex_mex.arf │ │ │ ├── ExtractDistanceTrajectory_terminate.c │ │ │ ├── ExtractDistanceTrajectory_terminate.h │ │ │ ├── ExtractDistanceTrajectory_types.h │ │ │ ├── RotateTrajectory.c │ │ │ ├── RotateTrajectory.h │ │ │ ├── buildInfo.mat │ │ │ ├── c_mexapi_version.c │ │ │ ├── eml_int_forloop_overflow_check.c │ │ │ ├── eml_int_forloop_overflow_check.h │ │ │ ├── error.c │ │ │ ├── error.h │ │ │ ├── interface │ │ │ │ ├── _coder_ExtractDistanceTrajectory_api.c │ │ │ │ ├── _coder_ExtractDistanceTrajectory_api.h │ │ │ │ ├── _coder_ExtractDistanceTrajectory_info.c │ │ │ │ ├── _coder_ExtractDistanceTrajectory_info.h │ │ │ │ ├── _coder_ExtractDistanceTrajectory_mex.c │ │ │ │ └── _coder_ExtractDistanceTrajectory_mex.h │ │ │ ├── mod.c │ │ │ ├── mod.h │ │ │ ├── power.c │ │ │ ├── power.h │ │ │ ├── repmat.c │ │ │ ├── repmat.h │ │ │ ├── rt_nonfinite.h │ │ │ ├── rtwtypes.h │ │ │ ├── scalexpAlloc.c │ │ │ ├── scalexpAlloc.h │ │ │ ├── setEnv.bat │ │ │ ├── sort1.c │ │ │ ├── sort1.h │ │ │ ├── sortIdx.c │ │ │ └── sortIdx.h │ │ │ ├── ExtractWindowTrajectory │ │ │ ├── ExtractWindowTrajectory.c │ │ │ ├── ExtractWindowTrajectory.h │ │ │ ├── ExtractWindowTrajectory_data.c │ │ │ ├── ExtractWindowTrajectory_data.h │ │ │ ├── ExtractWindowTrajectory_emxutil.c │ │ │ ├── ExtractWindowTrajectory_emxutil.h │ │ │ ├── ExtractWindowTrajectory_initialize.c │ │ │ ├── ExtractWindowTrajectory_initialize.h │ │ │ ├── ExtractWindowTrajectory_mex.bat │ │ │ ├── ExtractWindowTrajectory_mex.exp │ │ │ ├── ExtractWindowTrajectory_mex.mexw64 │ │ │ ├── ExtractWindowTrajectory_mex.mexw64.manifest │ │ │ ├── ExtractWindowTrajectory_mex.mk │ │ │ ├── ExtractWindowTrajectory_mex.mki │ │ │ ├── ExtractWindowTrajectory_mex_mex.arf │ │ │ ├── ExtractWindowTrajectory_terminate.c │ │ │ ├── ExtractWindowTrajectory_terminate.h │ │ │ ├── ExtractWindowTrajectory_types.h │ │ │ ├── RotateTrajectory.c │ │ │ ├── RotateTrajectory.h │ │ │ ├── abs.c │ │ │ ├── abs.h │ │ │ ├── assertValidSizeArg.c │ │ │ ├── assertValidSizeArg.h │ │ │ ├── buildInfo.mat │ │ │ ├── c_mexapi_version.c │ │ │ ├── diff.c │ │ │ ├── diff.h │ │ │ ├── eml_int_forloop_overflow_check.c │ │ │ ├── eml_int_forloop_overflow_check.h │ │ │ ├── error.c │ │ │ ├── error.h │ │ │ ├── ifWhileCond.c │ │ │ ├── ifWhileCond.h │ │ │ ├── indexShapeCheck.c │ │ │ ├── indexShapeCheck.h │ │ │ ├── interface │ │ │ │ ├── _coder_ExtractWindowTrajectory_api.c │ │ │ │ ├── _coder_ExtractWindowTrajectory_api.h │ │ │ │ ├── _coder_ExtractWindowTrajectory_info.c │ │ │ │ ├── _coder_ExtractWindowTrajectory_info.h │ │ │ │ ├── _coder_ExtractWindowTrajectory_mex.c │ │ │ │ └── _coder_ExtractWindowTrajectory_mex.h │ │ │ ├── power.c │ │ │ ├── power.h │ │ │ ├── repmat.c │ │ │ ├── repmat.h │ │ │ ├── rt_nonfinite.h │ │ │ ├── rtwtypes.h │ │ │ ├── scalexpAlloc.c │ │ │ ├── scalexpAlloc.h │ │ │ ├── setEnv.bat │ │ │ ├── sort1.c │ │ │ ├── sort1.h │ │ │ ├── sortIdx.c │ │ │ └── sortIdx.h │ │ │ └── FitReferencePathPolynomial │ │ │ ├── ComputeDerivativePolynomialCoefficients.c │ │ │ ├── ComputeDerivativePolynomialCoefficients.h │ │ │ ├── ComputeSquaredPolynomialCoefficients.c │ │ │ ├── ComputeSquaredPolynomialCoefficients.h │ │ │ ├── ConstrainedLeastSquares.c │ │ │ ├── ConstrainedLeastSquares.h │ │ │ ├── ConstrainedPolyFit.c │ │ │ ├── ConstrainedPolyFit.h │ │ │ ├── EvaluatePolynomial.c │ │ │ ├── EvaluatePolynomial.h │ │ │ ├── FitReferencePathPolynomial.c │ │ │ ├── FitReferencePathPolynomial.h │ │ │ ├── FitReferencePathPolynomial_data.c │ │ │ ├── FitReferencePathPolynomial_data.h │ │ │ ├── FitReferencePathPolynomial_emxutil.c │ │ │ ├── FitReferencePathPolynomial_emxutil.h │ │ │ ├── FitReferencePathPolynomial_initialize.c │ │ │ ├── FitReferencePathPolynomial_initialize.h │ │ │ ├── FitReferencePathPolynomial_mex.bat │ │ │ ├── FitReferencePathPolynomial_mex.exp │ │ │ ├── FitReferencePathPolynomial_mex.mexw64 │ │ │ ├── FitReferencePathPolynomial_mex.mexw64.manifest │ │ │ ├── FitReferencePathPolynomial_mex.mk │ │ │ ├── FitReferencePathPolynomial_mex.mki │ │ │ ├── FitReferencePathPolynomial_mex_mex.arf │ │ │ ├── FitReferencePathPolynomial_mexutil.c │ │ │ ├── FitReferencePathPolynomial_mexutil.h │ │ │ ├── FitReferencePathPolynomial_terminate.c │ │ │ ├── FitReferencePathPolynomial_terminate.h │ │ │ ├── FitReferencePathPolynomial_types.h │ │ │ ├── buildInfo.mat │ │ │ ├── c_mexapi_version.c │ │ │ ├── diag.c │ │ │ ├── diag.h │ │ │ ├── diff.c │ │ │ ├── diff.h │ │ │ ├── eml_int_forloop_overflow_check.c │ │ │ ├── eml_int_forloop_overflow_check.h │ │ │ ├── error.c │ │ │ ├── error.h │ │ │ ├── error1.c │ │ │ ├── error1.h │ │ │ ├── interface │ │ │ ├── _coder_FitReferencePathPolynomial_api.c │ │ │ ├── _coder_FitReferencePathPolynomial_api.h │ │ │ ├── _coder_FitReferencePathPolynomial_info.c │ │ │ ├── _coder_FitReferencePathPolynomial_info.h │ │ │ ├── _coder_FitReferencePathPolynomial_mex.c │ │ │ └── _coder_FitReferencePathPolynomial_mex.h │ │ │ ├── linspace.c │ │ │ ├── linspace.h │ │ │ ├── power.c │ │ │ ├── power.h │ │ │ ├── rdivide.c │ │ │ ├── rdivide.h │ │ │ ├── rt_nonfinite.h │ │ │ ├── rtwtypes.h │ │ │ ├── scalexpAlloc.c │ │ │ ├── scalexpAlloc.h │ │ │ ├── setEnv.bat │ │ │ ├── svd.c │ │ │ ├── svd.h │ │ │ ├── svd1.c │ │ │ └── svd1.h │ │ ├── kugle_mpc_obstacles.mexw64 │ │ ├── kugle_mpc_obstacles_codegen.cpp │ │ ├── kugle_mpc_obstacles_codegen.mexw64 │ │ ├── kugle_mpc_obstacles_codegen │ │ ├── Makefile │ │ ├── acado_auxiliary_functions.c │ │ ├── acado_auxiliary_functions.h │ │ ├── acado_common.h │ │ ├── acado_indices.h │ │ ├── acado_integrator.c │ │ ├── acado_qpoases3_interface.c │ │ ├── acado_qpoases3_interface.h │ │ ├── acado_solver.c │ │ ├── acado_solver.mexw64 │ │ ├── acado_solver_mex.c │ │ ├── make_acado_solver.m │ │ ├── qpoases3 │ │ │ ├── AUTHORS │ │ │ ├── AUTHORS.txt │ │ │ ├── INSTALL │ │ │ ├── INSTALL.txt │ │ │ ├── LICENSE.txt │ │ │ ├── Makefile │ │ │ ├── README │ │ │ ├── README.txt │ │ │ ├── VERSIONS │ │ │ ├── VERSIONS.txt │ │ │ ├── bin │ │ │ │ └── empty.txt │ │ │ ├── doc │ │ │ │ ├── Makefile │ │ │ │ ├── doxygen.config │ │ │ │ └── mainpage.dox │ │ │ ├── examples │ │ │ │ ├── Makefile │ │ │ │ ├── example1.c │ │ │ │ ├── example1b.c │ │ │ │ ├── example3.c │ │ │ │ └── exampleLP.c │ │ │ ├── include │ │ │ │ ├── qpOASES_e.h │ │ │ │ └── qpOASES_e │ │ │ │ │ ├── Bounds.h │ │ │ │ │ ├── Constants.h │ │ │ │ │ ├── ConstraintProduct.h │ │ │ │ │ ├── Constraints.h │ │ │ │ │ ├── Flipper.h │ │ │ │ │ ├── Indexlist.h │ │ │ │ │ ├── Matrices.h │ │ │ │ │ ├── MessageHandling.h │ │ │ │ │ ├── Options.h │ │ │ │ │ ├── QProblem.h │ │ │ │ │ ├── QProblemB.h │ │ │ │ │ ├── Types.h │ │ │ │ │ ├── UnitTesting.h │ │ │ │ │ ├── Utils.h │ │ │ │ │ └── extras │ │ │ │ │ └── OQPinterface.h │ │ │ ├── interfaces │ │ │ │ ├── matlab │ │ │ │ │ ├── example1.mat │ │ │ │ │ ├── example1a.mat │ │ │ │ │ ├── example1b.mat │ │ │ │ │ ├── make.m │ │ │ │ │ ├── qpOASES_e.c │ │ │ │ │ ├── qpOASES_e.m │ │ │ │ │ ├── qpOASES_e_auxInput.m │ │ │ │ │ ├── qpOASES_e_matlab_utils.c │ │ │ │ │ ├── qpOASES_e_matlab_utils.h │ │ │ │ │ ├── qpOASES_e_options.m │ │ │ │ │ ├── qpOASES_e_sequence.c │ │ │ │ │ └── qpOASES_e_sequence.m │ │ │ │ └── simulink │ │ │ │ │ ├── example_QProblem.mdl │ │ │ │ │ ├── example_QProblemB.mdl │ │ │ │ │ ├── load_example_QProblem.m │ │ │ │ │ ├── load_example_QProblemB.m │ │ │ │ │ ├── make.m │ │ │ │ │ ├── qpOASES_e_QProblem.c │ │ │ │ │ ├── qpOASES_e_QProblemB.c │ │ │ │ │ └── qpOASES_e_simulink_utils.c │ │ │ ├── make.mk │ │ │ ├── make_cygwin.mk │ │ │ ├── make_linux.mk │ │ │ ├── make_osx.mk │ │ │ ├── make_windows.mk │ │ │ ├── src │ │ │ │ ├── Bounds.c │ │ │ │ ├── Constraints.c │ │ │ │ ├── Flipper.c │ │ │ │ ├── Indexlist.c │ │ │ │ ├── Makefile │ │ │ │ ├── Matrices.c │ │ │ │ ├── MessageHandling.c │ │ │ │ ├── OQPinterface.c │ │ │ │ ├── Options.c │ │ │ │ ├── QProblem.c │ │ │ │ ├── QProblemB.c │ │ │ │ └── Utils.c │ │ │ └── testing │ │ │ │ └── c │ │ │ │ ├── Makefile │ │ │ │ ├── data │ │ │ │ └── fetch_cpp_data │ │ │ │ ├── test_bench.c │ │ │ │ └── test_matrices.c │ │ └── test.c │ │ ├── kugle_mpc_obstacles_codegen_RUN.m │ │ ├── kugle_mpc_obstacles_codegen_RUN.mexw64 │ │ ├── kugle_mpc_obstacles_codegen_data_acadodata_M1.txt │ │ ├── kugle_mpc_obstacles_codegen_data_acadodata_M2.txt │ │ ├── kugle_mpc_obstacles_codegen_data_acadodata_V1.txt │ │ └── kugle_mpc_obstacles_codegen_data_acadodata_V2.txt ├── SlidingMode │ └── SlidingModeControl.m └── VelocityLQR │ ├── GainComputation.m │ ├── VelocityLQR.m │ └── generated │ └── VelocityLQRgain.mat ├── DataProcessing ├── README.md ├── VisualizeDump.m └── functions │ ├── Downsample.m │ ├── DumpViconTimeSynchronization.m │ ├── LoadDump.m │ ├── LoadSimulation.m │ ├── LoadVicon.m │ ├── ParseCovarianceDumpArray.m │ ├── ParseDumpArray.m │ ├── ParseSensorDumpArray.m │ ├── QuaternionYawOffset.m │ ├── RemoveTime.m │ ├── Trim.m │ ├── TrimSynced.m │ ├── csvwrite_with_headers.m │ └── getlatestfile.m ├── Estimators ├── Madgwick │ ├── @MadgwickAHRS │ │ └── MadgwickAHRS.m │ ├── @MahonyAHRS │ │ └── MahonyAHRS.m │ ├── ExampleScript.m │ ├── LICENSE │ └── quaternion_library │ │ ├── TestScript.m │ │ ├── axisAngle2quatern.m │ │ ├── axisAngle2rotMat.m │ │ ├── euler2rotMat.m │ │ ├── quatern2euler.m │ │ ├── quatern2rotMat.m │ │ ├── quaternConj.m │ │ ├── quaternProd.m │ │ ├── rotMat2euler.m │ │ └── rotMat2quatern.m ├── QEKF │ ├── Codegen.m │ ├── QEKF.m │ └── codegen │ │ └── lib │ │ └── QEKF │ │ ├── QEKF.cpp │ │ ├── QEKF.h │ │ ├── QEKF_initialize.cpp │ │ ├── QEKF_initialize.h │ │ ├── QEKF_ref.rsp │ │ ├── QEKF_rtw.bat │ │ ├── QEKF_rtw.mk │ │ ├── QEKF_terminate.cpp │ │ ├── QEKF_terminate.h │ │ ├── QEKF_types.h │ │ ├── buildInfo.mat │ │ ├── codeInfo.mat │ │ ├── codedescriptor.dmr │ │ ├── examples │ │ ├── main.cpp │ │ └── main.h │ │ ├── eye.cpp │ │ ├── eye.h │ │ ├── html │ │ └── report.mldatx │ │ ├── interface │ │ ├── _coder_QEKF_api.c │ │ ├── _coder_QEKF_api.h │ │ ├── _coder_QEKF_info.c │ │ ├── _coder_QEKF_info.h │ │ ├── _coder_QEKF_mex.cpp │ │ └── _coder_QEKF_mex.h │ │ ├── mrdivide.cpp │ │ ├── mrdivide.h │ │ ├── mw_cmsis.h │ │ ├── norm.cpp │ │ ├── norm.h │ │ ├── rtGetInf.cpp │ │ ├── rtGetInf.h │ │ ├── rtGetNaN.cpp │ │ ├── rtGetNaN.h │ │ ├── rt_defines.h │ │ ├── rt_nonfinite.cpp │ │ ├── rt_nonfinite.h │ │ ├── rtw_proj.tmw │ │ └── rtwtypes.h └── VEKF │ ├── Codegen.m │ ├── VelocityEKF.m │ └── codegen │ └── lib │ └── VelocityEKF │ ├── VelocityEKF.cpp │ ├── VelocityEKF.h │ ├── VelocityEKF_initialize.cpp │ ├── VelocityEKF_initialize.h │ ├── VelocityEKF_ref.rsp │ ├── VelocityEKF_rtw.bat │ ├── VelocityEKF_rtw.mk │ ├── VelocityEKF_terminate.cpp │ ├── VelocityEKF_terminate.h │ ├── VelocityEKF_types.h │ ├── buildInfo.mat │ ├── codeInfo.mat │ ├── codedescriptor.dmr │ ├── examples │ ├── main.cpp │ └── main.h │ ├── html │ └── report.mldatx │ ├── interface │ ├── _coder_VelocityEKF_api.c │ ├── _coder_VelocityEKF_api.h │ ├── _coder_VelocityEKF_info.c │ ├── _coder_VelocityEKF_info.h │ ├── _coder_VelocityEKF_mex.cpp │ └── _coder_VelocityEKF_mex.h │ ├── mrdivide.cpp │ ├── mrdivide.h │ ├── rtGetInf.cpp │ ├── rtGetInf.h │ ├── rtGetNaN.cpp │ ├── rtGetNaN.h │ ├── rt_nonfinite.cpp │ ├── rt_nonfinite.h │ ├── rtw_proj.tmw │ └── rtwtypes.h ├── Kugle - Modelling and Control of a Ball-balancing Robot.pdf ├── Kugle - Presentation slides.pdf ├── LICENSE ├── Linearization ├── ClosedLoopLinearization_SlidingMode.m ├── ClosedLoopLinearization_SlidingMode_WithOmegaRef.m ├── ModelLinearization.m └── generated │ ├── ClosedLoopModelMatrices_SlidingMode.mat │ ├── ClosedLoopModelMatrices_SlidingMode_WithOmegaRef.mat │ ├── LinearizedModelMatrices.mat │ └── SteadyStateAccelerationConstants.mat ├── MeasurementDataset ├── PlotVicon.m ├── README.md ├── SineReferenceGeneration.m ├── Test1 │ ├── Processing.m │ ├── Processing_OnlyTilt.m │ └── VerifyViconOrigin.m ├── Test10 │ ├── Processing.m │ ├── hline.m │ └── vline.m ├── Test11 │ ├── Processing.m │ ├── hline.m │ └── vline.m ├── Test12 │ ├── Processing.m │ ├── hline.m │ └── vline.m ├── Test13 │ └── Processing.m ├── Test14 │ └── Processing.m ├── Test15 │ └── Processing.m ├── Test16 │ └── Processing.m ├── Test17 │ └── Processing.m ├── Test19 │ └── Processing.m ├── Test1_Fast │ └── Processing.m ├── Test2 │ └── Processing.m ├── Test20 │ └── Processing.m ├── Test24 │ └── Processing.m ├── Test25 │ └── Processing.m ├── Test26 │ ├── Processing.m │ ├── hline.m │ └── vline.m ├── Test27 │ └── Processing.m ├── Test28 │ └── Processing.m ├── Test3 │ ├── HeadingInputTest.m │ └── Processing.m ├── Test4 │ └── Processing.m ├── Test5 │ └── Processing.m ├── Test6 │ └── Processing.m ├── Test7 │ └── Processing.m ├── Test7_2 │ └── Processing.m ├── Test8 │ └── Processing.m ├── Test9 │ ├── Processing.m │ └── WithoutController.m ├── VisualizeDump.m └── vicon_data_acquisition.m ├── Misc ├── ExtractHeading.m ├── HeadingIndependentReference.m ├── HeadingQuaternion.m ├── MEX tests │ ├── ACADO_MPC_MEX.c │ ├── README.md │ ├── mexHelloWorld.c │ ├── mexcpp_example.cpp │ └── persistant_test.c ├── Quaternion2ZYXEulerAngles.m ├── QuaternionError.m ├── ReplaceStringInFile.m ├── ZYXEulerAngles2Quaternion.m ├── omega_b2qdot.m └── qdot2omega_b.m ├── Model ├── EulerLagrange_PlanarModelDerivation.m ├── EulerLagrange_QuaternionModelDerivation.m ├── EvaluateCompleteODE.m ├── ForwardKinematicsDerivation.m ├── SteadyStateAccelerationDerivation.m ├── functions │ └── SimplifyWithQuatConstraint.m └── generated │ ├── ForwardKinematics.m │ ├── ForwardKinematicsInHeadingFrame.m │ ├── ForwardKinematics_2L.m │ ├── ForwardKinematics_COM.m │ ├── ForwardKinematics_WithRot.m │ ├── InverseKinematics.m │ ├── PlanarInverseKinematics.m │ ├── SteadyStateAcceleration.m │ ├── SymbolicModel.mat │ ├── coriolis.m │ ├── friction.m │ ├── gravity.m │ ├── input_forces.m │ ├── mass.m │ ├── planar_A_matrix.m │ ├── planar_B_matrix.m │ ├── planar_coriolis.m │ ├── planar_friction.m │ ├── planar_gravity.m │ ├── planar_input_forces.m │ └── planar_mass.m ├── Parameters ├── Constants_Kugle.m ├── Constants_nominal.m ├── Parameters_Controllers.m ├── Parameters_Estimators.m ├── Parameters_General.m ├── Parameters_MPC.m └── Parameters_Simulation.m ├── README.md ├── Simulation ├── KugleSim_LQR.slx ├── KugleSim_MPC.slx ├── KugleSim_SlidingMode.slx ├── KugleSim_VelocityLQR.slx ├── MPC_Simulation.slx └── subsystems │ ├── Balance_LQR.slx │ ├── Estimators.slx │ ├── KinematicsIntegration.slx │ ├── KugleModel.slx │ ├── Logging.slx │ ├── MPC.slx │ ├── Madgwick.slx │ ├── PrepareMPCtrajectory.m │ ├── QEKF_.slx │ ├── ReferenceGenerator.slx │ ├── SensorModels.slx │ ├── SlidingMode.slx │ ├── StatesComparison.slx │ ├── SteadyStateAccelerationModel.slx │ ├── StructurizeLoggedData.m │ ├── TrackingComparison.slx │ ├── VEKF.slx │ ├── Velocity_LQR.slx │ └── qlib.mdl ├── control_loop_structure.png ├── estimators.png ├── kugle_front.jpg ├── kugle_v1_3d.png ├── kugle_v1_drawing.png ├── mpc_loop.png ├── sensormodels.png ├── simulink_top_level.png ├── system_architecture.png └── velocity_control_loop_structure.png /.gitignore: -------------------------------------------------------------------------------- 1 | # Simulink cache files 2 | *.slxc 3 | 4 | # Simulink temp project folder 5 | slprj/ 6 | 7 | # MATLAB autosave/temp files 8 | *.asv 9 | 10 | # Simulink autosave files 11 | *.autosave -------------------------------------------------------------------------------- /Codegen/CodegenExample.m: -------------------------------------------------------------------------------- 1 | % Example script to illustrate how to convert functions with MATLAB Coder 2 | % Note that conversion with MATLAB coder requires the "ARM Cortex-M" code 3 | % replacement library to be installed, found within the "Embedded Coder 4 | % Support Package for ARM Cortex-M Processors" 5 | % https://se.mathworks.com/matlabcentral/fileexchange/43095-embedded-coder-support-package-for-arm-cortex-m-processors 6 | %load('CoderConfig_ARM_CortexM.mat'); 7 | load('config.mat'); 8 | 9 | % Define inputs 10 | A = coder.typeof(single(0),[3 3],0); % fixed size of floats 11 | 12 | % Try changing the flag below - it does not affect generated code 13 | config.RowMajor = true; 14 | %config.RowMajor = false; 15 | 16 | % Convert desired function (in this case 'svd') 17 | codegen svd -config config -nargout 3 -args { A } -------------------------------------------------------------------------------- /Codegen/CodegenToMEX.m: -------------------------------------------------------------------------------- 1 | % Example script to illustrate how to optimize functions with MATLAB Coder 2 | % by converting them into MEX 3 | 4 | % Define inputs 5 | A = coder.typeof(single(0),[3 3],0); % fixed size of floats 6 | 7 | % Convert desired function to MEX (in this case 'svd') 8 | codegen svd -nargout 3 -args { A } -------------------------------------------------------------------------------- /Codegen/CoderConfig_ARM_CortexM.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mindThomas/Kugle-MATLAB/b1ee16de78899a131241639d620b5fb7c02a586b/Codegen/CoderConfig_ARM_CortexM.mat -------------------------------------------------------------------------------- /Controllers/BalanceLQR/generated/BalanceLQRgain.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mindThomas/Kugle-MATLAB/b1ee16de78899a131241639d620b5fb7c02a586b/Controllers/BalanceLQR/generated/BalanceLQRgain.mat -------------------------------------------------------------------------------- /Controllers/MPC/EvaluateLinearMPCmodel.m: -------------------------------------------------------------------------------- 1 | function dX = EvaluateLinearMPCmodel(t, X, u, AccelerationConstant_q3_to_ddx, AccelerationConstant_q2_to_ddy) 2 | % Extract states 3 | q1 = X(1); 4 | q2 = X(2); 5 | q3 = X(3); 6 | q4 = X(4); 7 | x = X(5); 8 | y = X(6); 9 | dx = X(7); 10 | dy = X(8); 11 | 12 | q = [q1,q2,q3,q4]'; 13 | 14 | % Extract inputs 15 | omeg_ref_x = u(1); 16 | omeg_ref_y = u(2); 17 | 18 | % Non-linear ODE/model equations with omega_ref given in body frame 19 | dq2 = 1/2 * omeg_ref_x; 20 | dq3 = 1/2 * omeg_ref_y; 21 | 22 | % OBS. The linear MPC model does not include yaw turning 23 | dq = [0,dq2,dq3,0]'; 24 | 25 | ddx = AccelerationConstant_q3_to_ddx*q3; 26 | ddy = AccelerationConstant_q2_to_ddy*q2; 27 | 28 | dX = [dq; dx; dy; ddx; ddy]; -------------------------------------------------------------------------------- /Controllers/MPC/ExtractDistanceTrajectory_mex.mexw64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mindThomas/Kugle-MATLAB/b1ee16de78899a131241639d620b5fb7c02a586b/Controllers/MPC/ExtractDistanceTrajectory_mex.mexw64 -------------------------------------------------------------------------------- /Controllers/MPC/TestConstrainedLeastSquared.m: -------------------------------------------------------------------------------- 1 | scriptDir = fileparts(mfilename('fullpath')); 2 | addpath(fullfile(scriptDir, 'functions')); 3 | 4 | % Test parameters 5 | coeff_a = 2; 6 | coeff_b = -15; 7 | sigma = 10; 8 | 9 | % Make test line with known relationship 10 | x = linspace(0, 100, 101)'; 11 | y = coeff_a*x + coeff_b; 12 | 13 | % Add noise 14 | y = y + sigma*randn(length(x),1); 15 | 16 | %% Perform least squares to estimate a and b 17 | % Let the unknown parameter vector be defined as: 18 | % x = [a,b] 19 | n = length(x); 20 | A = [x, ones(n,1)]; 21 | b = y; 22 | Aeq = [0,1]; % test with line start should be in zero, hence requiring b=0 23 | beq = 0; 24 | params = ConstrainedLeastSquares(A, b, Aeq, beq, n*1000); 25 | 26 | % Test the fit 27 | y_fit = A * params; 28 | 29 | % Compute estimation error 30 | estimation_error = params - [coeff_a;coeff_b] 31 | 32 | % Visualize line and fit 33 | figure(1); 34 | plot(x, y); 35 | hold on; 36 | plot(x, y_fit); 37 | hold off; 38 | 39 | ylim([-50, 250]); -------------------------------------------------------------------------------- /Controllers/MPC/functions/ConstrainedLeastSquares.m: -------------------------------------------------------------------------------- 1 | % x = ConstrainedLeastSquares(A, b, Aeq, beq, lambda) 2 | % 3 | % Fit linear function 4 | % c2*x^2 + c1*x + c0 = y 5 | % Perform through least squares as: min |Ax - y|^2 6 | % x = [c2, c1, c0]' 7 | % A = [x1^2, x1, 1; x2^2, x2, 1; ...]; 8 | % Which is the same as using the pseudo-inverse of A: A^+ 9 | % x_hat = A^+ y 10 | function x = ConstrainedLeastSquares(A, b, Aeq, beq, lambda) 11 | 12 | if (size(Aeq,1) == size(beq,1) && size(A,2) == size(Aeq,2)) 13 | A_ = [A; lambda*Aeq]; 14 | b_ = [b; lambda*beq]; 15 | else 16 | A_ = A; 17 | b_ = b; 18 | end 19 | 20 | % Pseudo-inverse through numerically unstable way 21 | %A_invpseudo = inv(A_'*A_) * A_' 22 | % Pseudo-inverse through MATLAB 23 | %A_invpseudo2 = pinv(A_) % Moore-Penrose Pseudoinverse of matrix of A 24 | % Pseudo-inverse through SVD 25 | [U,S,V] = svd(A_); 26 | Sinv = [diag(1./diag(S)), zeros(size(S,2), size(S,1)-size(S,2))]; 27 | A_invpseudo3 = V * Sinv * U'; 28 | 29 | x = A_invpseudo3 * b_; -------------------------------------------------------------------------------- /Controllers/MPC/functions/DefineWeightedStates.m: -------------------------------------------------------------------------------- 1 | function h = DefineWeightedStates(statesString) 2 | global ACADO_; 3 | 4 | statesString = statesString(~isspace(statesString)); 5 | states = strsplit(statesString, ';'); 6 | 7 | ACADO_.y = states; 8 | h = evalin('base', ['[' statesString ']']); 9 | end -------------------------------------------------------------------------------- /Controllers/MPC/functions/DefineWeightedTerminalStates.m: -------------------------------------------------------------------------------- 1 | function hN = DefineWeightedTerminalStates(statesString) 2 | global ACADO_; 3 | 4 | statesString = statesString(~isspace(statesString)); 5 | states = strsplit(statesString, ';'); 6 | 7 | ACADO_.yN = states; 8 | hN = evalin('base', ['[' statesString ']']); 9 | end -------------------------------------------------------------------------------- /Controllers/MPC/functions/polynomial/ArcLengthApproximation.m: -------------------------------------------------------------------------------- 1 | % Based on http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.5.2912&rep=rep1&type=pdf 2 | function s = ArcLengthApproximation(coeff_x, coeff_y, t) %#codegen 3 | % Arc length is given as: 4 | % integral( sqrt( dxdt^2 + dydt^2 ) ) dt 5 | % This can however be approximated by using the velocity polynomial 6 | % Q = sqrt( (df_x/dx)^2 + (df_dy)^2 ); 7 | % First we compute the coefficients of the inner polynomial, f 8 | % f = (df_x/dx)^2 + (df_dy)^2 9 | dx_coeff = ComputeDerivativePolynomialCoefficients(coeff_x); % taking the difference of a polynomial, moves the coefficients 10 | dy_coeff = ComputeDerivativePolynomialCoefficients(coeff_y); 11 | dx_squared_coeff = ComputeSquaredPolynomialCoefficients(dx_coeff); 12 | dy_squared_coeff = ComputeSquaredPolynomialCoefficients(dy_coeff); 13 | f_coeff = dx_squared_coeff + dy_squared_coeff; 14 | % Such that 15 | % Q = sqrt(EvaluatePolynomial(f_coeff, t)) 16 | 17 | % Now the approximation (see paper above) is given by 18 | % s(t) = t/2 * (5/9 * Q(1.774597*t/2) + 8/9 * Q(t/2) + 5/9 * Q(0.225403*t/2)) 19 | s = t/2 .* (5/9 * sqrt(EvaluatePolynomial(f_coeff, 1.774597*t/2)) + 8/9 * sqrt(EvaluatePolynomial(f_coeff, t/2)) + 5/9 * sqrt(EvaluatePolynomial(f_coeff, 0.225403*t/2))); -------------------------------------------------------------------------------- /Controllers/MPC/functions/polynomial/ArcLengthApproximationNumerical.m: -------------------------------------------------------------------------------- 1 | % Numerical integration to compute arc curve length 2 | function s = ArcLengthApproximationNumerical(coeff_x, coeff_y, t, precision) 3 | % Arc length is given as: 4 | % integral( sqrt( dxdt^2 + dydt^2 ) ) dt 5 | % First we compute the coefficients of the inner polynomial, f 6 | % f = (df_x/dx)^2 + (df_dy)^2 7 | dx_coeff = ComputeDerivativePolynomialCoefficients(coeff_x); % taking the difference of a polynomial, moves the coefficients 8 | dy_coeff = ComputeDerivativePolynomialCoefficients(coeff_y); 9 | dx_squared_coeff = ComputeSquaredPolynomialCoefficients(dx_coeff); 10 | dy_squared_coeff = ComputeSquaredPolynomialCoefficients(dy_coeff); 11 | f_coeff = dx_squared_coeff + dy_squared_coeff; 12 | 13 | % Now compute arc curve length by performing numerical integration 14 | t_ = (0:precision:t)'; 15 | y = sqrt(EvaluatePolynomial(f_coeff, t_)); 16 | % Perform trapezoidal integration 17 | y = [y;0]; 18 | % s[k] = s[k-1] + dt * (f[k-1] + f[k]) / 2 19 | yT = precision * (y(1:end-1) + y(2:end)) / 2; 20 | integrate = cumsum(yT); 21 | s = integrate(end); -------------------------------------------------------------------------------- /Controllers/MPC/functions/polynomial/ComputeDerivativePolynomialCoefficients.m: -------------------------------------------------------------------------------- 1 | % Given polynomial coefficients for: 2 | % f(x) = c_n*x^n + c_n-1*x^(n-1) + ... c_1*x + c_0 3 | % This function computes the coefficients of the polynomial f'(x) 4 | % f'(x) = df/dx = n*c_n*x^(n-1) + (n-1)*c_n-1*x^(n-2) + ... + 5 | % Coefficients are ordered such that coeff(1) = c_n (highest order) 6 | function dcoeff = ComputeDerivativePolynomialCoefficients(coeff) 7 | n = length(coeff) - 1; 8 | dcoeff = (n:-1:1)'.*coeff(1:end-1); 9 | -------------------------------------------------------------------------------- /Controllers/MPC/functions/polynomial/ComputeSquaredPolynomialCoefficients.m: -------------------------------------------------------------------------------- 1 | % Given polynomial coefficients for: 2 | % f(x) = c_n*x^n + c_n-1*x^(n-1) + ... c_1*x + c_0 3 | % This function computes the coefficients of the polynomial f^2(x) 4 | % Coefficients are ordered such that coeff(1) = c_n (highest order) 5 | function coeff_squared = ComputeSquaredPolynomialCoefficients(coeff) %#codegen 6 | % f^2(x) = sum(sum(c_i * c_j * x^(i+j)) 7 | % The resulting coefficients, coeff_k = c_i*c_j | i+j=k 8 | n = length(coeff) - 1; % input polynomial order 9 | m = 2*n; % squared (output) polynomial order 10 | 11 | coeff_squared = zeros(m+1,1); 12 | for (k = 0:m) 13 | for (i = 0:n) 14 | for (j = 0:n) 15 | if (i+j == k) 16 | coeff_squared(m-k+1) = coeff_squared(m-k+1) + coeff(n-i+1)*coeff(n-j+1); 17 | end 18 | end 19 | end 20 | end -------------------------------------------------------------------------------- /Controllers/MPC/functions/polynomial/EvaluatePolynomial.m: -------------------------------------------------------------------------------- 1 | % Evaluate a polynomial ( similar to Y = polyval(P,X) ) 2 | function y = EvaluatePolynomial(coeff, x) %#codegen 3 | order = length(coeff) - 1; 4 | n = length(x); 5 | %y = (coeff' * (x.^(order:-1:0))')'; 6 | M = zeros(n, order+1); 7 | for (i = order:-1:0) 8 | M(:,order-i+1) = x .^ i; 9 | end 10 | y = M * coeff; -------------------------------------------------------------------------------- /Controllers/MPC/functions/polynomial/EvaluatePolynomialSymbolic.m: -------------------------------------------------------------------------------- 1 | % Evaluate a polynomial ( similar to Y = polyval(P,X) ) 2 | % Can also be used to create a symbolic polynomial expression from the coefficients and the symbolic variable x 3 | % But can not be used for code generation 4 | function y = EvaluatePolynomialSymbolic(coeff, x) 5 | order = length(coeff) - 1; 6 | y = (coeff' * (x.^(order:-1:0))')'; -------------------------------------------------------------------------------- /Controllers/MPC/functions/polynomial/PolynomialMinimumFinder.m: -------------------------------------------------------------------------------- 1 | % Find the minimum by Newton minimization 2 | function s_min = PolynomialMinimumFinder(coeff, s0, stoppingCriteria) 3 | dcoeff = ComputeDerivativePolynomialCoefficients(coeff); 4 | ddcoeff = ComputeDerivativePolynomialCoefficients(dcoeff); 5 | 6 | % Newton's method - https://en.wikipedia.org/wiki/Newton%27s_method_in_optimization 7 | converged = false; 8 | s = s0; 9 | while (~converged) 10 | FirstDerivative = EvaluatePolynomial(dcoeff, s); % first derivative, for higher dimensions it would be the Gradient 11 | SecondDerivative = EvaluatePolynomial(ddcoeff, s); % second derivative, for higher dimensions it would be the Hessian 12 | delta_s = -FirstDerivative / SecondDerivative; 13 | s = s + delta_s; 14 | 15 | if (abs(delta_s) < stoppingCriteria) 16 | converged = true; 17 | end 18 | end 19 | 20 | s_min = s; -------------------------------------------------------------------------------- /Controllers/MPC/functions/polynomial/PolynomialMinimumFinderNumerical.m: -------------------------------------------------------------------------------- 1 | % Find the minimum of a polynominal by brute-force line search 2 | function s_min = PolynomialMinimumFinderNumerical(coeff, s0, precision) 3 | span = 0.1; 4 | 5 | converged = false; 6 | while (~converged) 7 | s = (-span:precision:span)' + s0; 8 | y = EvaluatePolynomial(coeff, s); 9 | 10 | min = 1e99; 11 | s_min = 0; 12 | max = -1e99; 13 | s_max = 0; 14 | for (i = 1:length(s)) 15 | if (y(i) < min) 16 | min = y(i); 17 | s_min = s(i); 18 | end 19 | if (y(i) > max) 20 | max = y(i); 21 | s_max = s(i); 22 | end 23 | end 24 | 25 | if (s_min ~= 0 && y(1) > min && y(end) > min) 26 | converged = true; 27 | end 28 | span = span * 2; 29 | end 30 | 31 | plot(s, y); -------------------------------------------------------------------------------- /Controllers/MPC/functions/polynomial/PolynomialMinimumFinderNumerical2.m: -------------------------------------------------------------------------------- 1 | % Find the minimum of the derivative polynomial by brute-force line search 2 | function s_min = PolynomialMinimumFinderNumerical2(coeff, s0, precision) 3 | span = 0.1; 4 | dcoeff = ComputeDerivativePolynomialCoefficients(coeff); 5 | 6 | converged = false; 7 | while (~converged) 8 | s = (-span:precision:span)' + s0; 9 | y = EvaluatePolynomial(dcoeff, s) .^ 2; % we need to find a value as close to 0 as possible 10 | 11 | min = 1e99; 12 | s_min = 0; 13 | max = -1e99; 14 | s_max = 0; 15 | for (i = 1:length(s)) 16 | if (y(i) < min) 17 | min = y(i); 18 | s_min = s(i); 19 | end 20 | if (y(i) > max) 21 | max = y(i); 22 | s_max = s(i); 23 | end 24 | end 25 | 26 | if (s_min ~= 0 && y(1) > min && y(end) > min) 27 | converged = true; 28 | end 29 | span = span * 2; 30 | end 31 | 32 | plot(s, y); -------------------------------------------------------------------------------- /Controllers/MPC/functions/polynomial/PolynomialMinimumFinderWithBounds.m: -------------------------------------------------------------------------------- 1 | % Find the minimum by Newton minimization given upper and lower parameter bounds 2 | function s_min = PolynomialMinimumFinderWithBounds(coeff, s0, sLower, sUpper, stoppingCriteria, maxIterations) %#codegen 3 | dcoeff = ComputeDerivativePolynomialCoefficients(coeff); 4 | ddcoeff = ComputeDerivativePolynomialCoefficients(dcoeff); 5 | 6 | % Newton's method - https://en.wikipedia.org/wiki/Newton%27s_method_in_optimization 7 | converged = false; 8 | iterations = 0; 9 | s = s0; 10 | while (~converged && iterations < maxIterations) 11 | FirstDerivative = EvaluatePolynomial(dcoeff, s); % first derivative, for higher dimensions it would be the Gradient 12 | SecondDerivative = EvaluatePolynomial(ddcoeff, s); % second derivative, for higher dimensions it would be the Hessian 13 | delta_s = -FirstDerivative / SecondDerivative; 14 | s = s + delta_s; 15 | if (s < sLower) 16 | s = sLower; 17 | end 18 | if (s > sUpper) 19 | s = sUpper; 20 | end 21 | 22 | if (abs(delta_s) < stoppingCriteria) 23 | converged = true; 24 | end 25 | iterations = iterations + 1; 26 | end 27 | 28 | s_min = s; -------------------------------------------------------------------------------- /Controllers/MPC/functions/trajectory/GenerateTestTrajectory.m: -------------------------------------------------------------------------------- 1 | function [TrajectoryPoints, VisualizationLimits] = GenerateTestTrajectory() 2 | %% Generate test trajectory 3 | TrajectoryPoints = []; 4 | for (i = 0:(100+50+100+50-1)) 5 | if (i < 100) % straight x=10, y=-50...50 6 | p = [20, i-50]; 7 | elseif (i < 100+50) % left turn 180 degree with radius, r 8 | r = 20; 9 | p = [20-r,50] + r*[cos(pi/50*(i-100)), sin(pi/50*(i-100))]; 10 | elseif (i < 100+50+100) % straight x=-10, y=50...-50 11 | p = [-20, 50-(i-100-50)]; 12 | elseif (i < 100+50+100+50) % left turn 180 degree with radius, r 13 | r = 20; 14 | p = [20-r,-50] + r*[cos(pi/50*(i-100-50-100+50)), sin(pi/50*(i-100-50-100+50))]; 15 | end 16 | TrajectoryPoints = [TrajectoryPoints; p]; 17 | end 18 | 19 | TrajectoryPoints = [-TrajectoryPoints(:,2) TrajectoryPoints(:,1)]; % Rotate trajectory 90 degree 20 | TrajectoryPoints = TrajectoryPoints / 10; % downscale trajectory 21 | 22 | VisualizationLimits.x_min = -9; 23 | VisualizationLimits.y_min = -4; 24 | VisualizationLimits.x_max = 9; 25 | VisualizationLimits.y_max = 4; 26 | end -------------------------------------------------------------------------------- /Controllers/MPC/functions/trajectory/GenerateTestTrajectory_FigureEight.m: -------------------------------------------------------------------------------- 1 | function [TrajectoryPoints, VisualizationLimits] = GenerateTestTrajectory_FigureEight() 2 | %% Generate test trajectory 3 | TrajectoryPoints = []; 4 | for (i = 0:((50+270+100+270+50)-1)) 5 | if (i < 50) 6 | p = [i, i]/5; 7 | elseif (i < 50+270) 8 | r = sqrt(10^2+10^2); 9 | diag = sqrt(r^2+r^2); 10 | p = [diag,0] + r*[cos(pi/2+pi/4-2*pi*(i-50)/360),sin(pi/2+pi/4-2*pi*(i-50)/360)]; 11 | elseif (i < 50+270+100) 12 | p = [10,-10] + [-(i-50-270), (i-50-270)]/5; 13 | elseif (i < 50+270+100+270) 14 | r = sqrt(10^2+10^2); 15 | diag = sqrt(r^2+r^2); 16 | p = [-diag,0] + r*[cos(pi/4+2*pi*(i-50-270-100)/360),sin(pi/4+2*pi*(i-50-270-100)/360)]; 17 | elseif (i < 50+270+100+270+50) 18 | p = [-10,-10] + [(i-50-270-100-270), (i-50-270-100-270)]/5; 19 | end 20 | TrajectoryPoints = [TrajectoryPoints; p]; 21 | end 22 | 23 | TrajectoryPoints = TrajectoryPoints / 10; % downscale trajectory 24 | 25 | VisualizationLimits.x_min = -6; 26 | VisualizationLimits.y_min = -2; 27 | VisualizationLimits.x_max = 6; 28 | VisualizationLimits.y_max = 2; 29 | end -------------------------------------------------------------------------------- /Controllers/MPC/functions/trajectory/GetNearestObstacles.m: -------------------------------------------------------------------------------- 1 | function NearestObstacles = GetNearestObstacles(RobotPos, obstacles, number_of_nearest_obstacles) 2 | 3 | obstacles_pos = obstacles(:,1:2); 4 | obstacles_radius = obstacles(:,3); 5 | obstacles_vec = obstacles_pos - repmat(RobotPos, [size(obstacles_pos,1), 1]); 6 | obstacles_dist = sqrt(obstacles_vec(:,1).^2 + obstacles_vec(:,2).^2); 7 | obstacles_proximity = obstacles_dist - obstacles_radius; 8 | 9 | % Determine closest obstacles by sorting the proximity list 10 | [values, idx] = sort(obstacles_proximity); 11 | 12 | % Remove any obstacles which we are inside (e.g. due to random obstacles spawning on top of robot) 13 | %idx(find(obstacles_proximity(idx) < -1/4*obstacles_dist(idx))) = []; 14 | sortedIdx = []; 15 | for (i = 1:length(idx)) 16 | if (obstacles_proximity(idx(i)) >= -1/4*obstacles_dist(idx(i))) 17 | sortedIdx = [sortedIdx; idx(i)]; 18 | end 19 | end 20 | 21 | if (length(idx) > number_of_nearest_obstacles) 22 | NearestObstacles = obstacles(idx(1:number_of_nearest_obstacles),:); 23 | else 24 | NearestObstacles = obstacles(idx,:); 25 | end 26 | end -------------------------------------------------------------------------------- /Controllers/MPC/functions/trajectory/RotateTrajectory.m: -------------------------------------------------------------------------------- 1 | function rotatedPoints = RotateTrajectory(trajectory, rotation) %#codegen 2 | R = [cos(rotation), sin(rotation); 3 | -sin(rotation), cos(rotation)]; 4 | 5 | rotatedPoints = (R * trajectory')'; 6 | end -------------------------------------------------------------------------------- /Controllers/MPC/functions/visualization/PlotCircle.m: -------------------------------------------------------------------------------- 1 | function PlotCircle(center, radius) 2 | d = 2 * radius; 3 | px = center(1) - radius; 4 | py = center(1) - radius; 5 | rectangle('Position',[px py d d],'Curvature',[1,1],'EdgeColor',[1 0 0]); 6 | end -------------------------------------------------------------------------------- /Controllers/MPC/functions/visualization/PlotRobot.m: -------------------------------------------------------------------------------- 1 | function PlotRobot(center, yaw, radius) 2 | d = 2 * radius; 3 | px = center(1) - radius; 4 | py = center(2) - radius; 5 | rectangle('Position',[px py d d],'Curvature',[1,1],'EdgeColor',[1 0 0]); 6 | 7 | % Plot heading line 8 | h = center + radius*[cos(yaw) sin(yaw)]; 9 | line('XData',[center(1),h(1)], 'YData',[center(2),h(2)], 'Color',[1 0 0],'LineWidth',0.5); 10 | end -------------------------------------------------------------------------------- /Controllers/MPC/functions/visualization/PlotRobotWithVelocity.m: -------------------------------------------------------------------------------- 1 | function PlotRobotWithVelocity(center, yaw, radius, velocity) 2 | d = 2 * radius; 3 | px = center(1) - radius; 4 | py = center(2) - radius; 5 | rectangle('Position',[px py d d],'Curvature',[1,1],'EdgeColor',[1 0 0]); 6 | 7 | % Plot heading line 8 | h = center + radius*[cos(yaw) sin(yaw)]; 9 | line('XData',[center(1),h(1)], 'YData',[center(2),h(2)], 'Color',[1 0 0],'LineWidth',0.5); 10 | 11 | % Plot velocity line 12 | if (norm(velocity) > 0) 13 | if (length(velocity) == 1) % assuming velocity to be given in heading direction 14 | c = [center(1) center(2)] + radius*[cos(yaw) sin(yaw)]; 15 | h = c + 0.1*velocity*[cos(yaw) sin(yaw)]; 16 | line('XData',[c(1),h(1)], 'YData',[c(2),h(2)], 'Color',[0 0 1],'LineWidth',1.5); 17 | elseif (length(velocity) == 2) % 2d velocity given 18 | c = [center(1) center(2)] + radius * velocity/norm(velocity); 19 | h = c + 0.5*velocity; 20 | line('XData',[c(1),h(1)], 'YData',[c(2),h(2)], 'Color',[0 0 1],'LineWidth',1.5); 21 | end 22 | end 23 | end -------------------------------------------------------------------------------- /Controllers/MPC/generated/ExtractDistanceTrajectory_mex.mexw64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mindThomas/Kugle-MATLAB/b1ee16de78899a131241639d620b5fb7c02a586b/Controllers/MPC/generated/ExtractDistanceTrajectory_mex.mexw64 -------------------------------------------------------------------------------- /Controllers/MPC/generated/ExtractWindowTrajectory_mex.mexw64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mindThomas/Kugle-MATLAB/b1ee16de78899a131241639d620b5fb7c02a586b/Controllers/MPC/generated/ExtractWindowTrajectory_mex.mexw64 -------------------------------------------------------------------------------- /Controllers/MPC/generated/FitReferencePathPolynomial_mex.mexw64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mindThomas/Kugle-MATLAB/b1ee16de78899a131241639d620b5fb7c02a586b/Controllers/MPC/generated/FitReferencePathPolynomial_mex.mexw64 -------------------------------------------------------------------------------- /Controllers/MPC/generated/codegen/mex/ExtractDistanceTrajectory/ExtractDistanceTrajectory.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Academic License - for use in teaching, academic research, and meeting 3 | * course requirements at degree granting institutions only. Not for 4 | * government, commercial, or other organizational use. 5 | * 6 | * ExtractDistanceTrajectory.h 7 | * 8 | * Code generation for function 'ExtractDistanceTrajectory' 9 | * 10 | */ 11 | 12 | #ifndef EXTRACTDISTANCETRAJECTORY_H 13 | #define EXTRACTDISTANCETRAJECTORY_H 14 | 15 | /* Include files */ 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include "tmwtypes.h" 21 | #include "mex.h" 22 | #include "emlrt.h" 23 | #include "rtwtypes.h" 24 | #include "ExtractDistanceTrajectory_types.h" 25 | 26 | /* Function Declarations */ 27 | extern void ExtractDistanceTrajectory(const emlrtStack *sp, const 28 | emxArray_real_T *TrajectoryPoints, const real_T RobotPos[2], real_T RobotYaw, 29 | const real_T Velocity[2], real_T ExtractDist, real_T OrientationSelection, 30 | real_T PreviousClosestIndex, emxArray_real_T *WindowTrajectory, real_T 31 | *nTrajPoints, real_T *WindowOrientation, real_T *ClosestIdx); 32 | 33 | #endif 34 | 35 | /* End of code generation (ExtractDistanceTrajectory.h) */ 36 | -------------------------------------------------------------------------------- /Controllers/MPC/generated/codegen/mex/ExtractDistanceTrajectory/ExtractDistanceTrajectory_data.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Academic License - for use in teaching, academic research, and meeting 3 | * course requirements at degree granting institutions only. Not for 4 | * government, commercial, or other organizational use. 5 | * 6 | * ExtractDistanceTrajectory_data.h 7 | * 8 | * Code generation for function 'ExtractDistanceTrajectory_data' 9 | * 10 | */ 11 | 12 | #ifndef EXTRACTDISTANCETRAJECTORY_DATA_H 13 | #define EXTRACTDISTANCETRAJECTORY_DATA_H 14 | 15 | /* Include files */ 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include "tmwtypes.h" 21 | #include "mex.h" 22 | #include "emlrt.h" 23 | #include "rtwtypes.h" 24 | #include "ExtractDistanceTrajectory_types.h" 25 | 26 | /* Variable Declarations */ 27 | extern emlrtCTX emlrtRootTLSGlobal; 28 | extern emlrtContext emlrtContextGlobal; 29 | extern emlrtRSInfo l_emlrtRSI; 30 | extern emlrtRSInfo o_emlrtRSI; 31 | extern emlrtRSInfo lb_emlrtRSI; 32 | extern emlrtRSInfo ac_emlrtRSI; 33 | 34 | #endif 35 | 36 | /* End of code generation (ExtractDistanceTrajectory_data.h) */ 37 | -------------------------------------------------------------------------------- /Controllers/MPC/generated/codegen/mex/ExtractDistanceTrajectory/ExtractDistanceTrajectory_initialize.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Academic License - for use in teaching, academic research, and meeting 3 | * course requirements at degree granting institutions only. Not for 4 | * government, commercial, or other organizational use. 5 | * 6 | * ExtractDistanceTrajectory_initialize.c 7 | * 8 | * Code generation for function 'ExtractDistanceTrajectory_initialize' 9 | * 10 | */ 11 | 12 | /* Include files */ 13 | #include "rt_nonfinite.h" 14 | #include "ExtractDistanceTrajectory.h" 15 | #include "ExtractDistanceTrajectory_initialize.h" 16 | #include "_coder_ExtractDistanceTrajectory_mex.h" 17 | #include "ExtractDistanceTrajectory_data.h" 18 | 19 | /* Variable Definitions */ 20 | static const volatile char_T *emlrtBreakCheckR2012bFlagVar = NULL; 21 | 22 | /* Function Definitions */ 23 | void ExtractDistanceTrajectory_initialize(void) 24 | { 25 | emlrtStack st = { NULL, /* site */ 26 | NULL, /* tls */ 27 | NULL /* prev */ 28 | }; 29 | 30 | mexFunctionCreateRootTLS(); 31 | emlrtBreakCheckR2012bFlagVar = emlrtGetBreakCheckFlagAddressR2012b(); 32 | st.tls = emlrtRootTLSGlobal; 33 | emlrtClearAllocCountR2012b(&st, false, 0U, 0); 34 | emlrtEnterRtStackR2012b(&st); 35 | emlrtFirstTimeR2012b(emlrtRootTLSGlobal); 36 | } 37 | 38 | /* End of code generation (ExtractDistanceTrajectory_initialize.c) */ 39 | -------------------------------------------------------------------------------- /Controllers/MPC/generated/codegen/mex/ExtractDistanceTrajectory/ExtractDistanceTrajectory_initialize.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Academic License - for use in teaching, academic research, and meeting 3 | * course requirements at degree granting institutions only. Not for 4 | * government, commercial, or other organizational use. 5 | * 6 | * ExtractDistanceTrajectory_initialize.h 7 | * 8 | * Code generation for function 'ExtractDistanceTrajectory_initialize' 9 | * 10 | */ 11 | 12 | #ifndef EXTRACTDISTANCETRAJECTORY_INITIALIZE_H 13 | #define EXTRACTDISTANCETRAJECTORY_INITIALIZE_H 14 | 15 | /* Include files */ 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include "tmwtypes.h" 21 | #include "mex.h" 22 | #include "emlrt.h" 23 | #include "rtwtypes.h" 24 | #include "ExtractDistanceTrajectory_types.h" 25 | 26 | /* Function Declarations */ 27 | extern void ExtractDistanceTrajectory_initialize(void); 28 | 29 | #endif 30 | 31 | /* End of code generation (ExtractDistanceTrajectory_initialize.h) */ 32 | -------------------------------------------------------------------------------- /Controllers/MPC/generated/codegen/mex/ExtractDistanceTrajectory/ExtractDistanceTrajectory_mex.bat: -------------------------------------------------------------------------------- 1 | @echo off 2 | set MATLAB=C:\PROGRA~1\MATLAB\R2018a 3 | set MATLAB_ARCH=win64 4 | set MATLAB_BIN="C:\Program Files\MATLAB\R2018a\bin" 5 | set ENTRYPOINT=mexFunction 6 | set OUTDIR=.\ 7 | set LIB_NAME=ExtractDistanceTrajectory_mex 8 | set MEX_NAME=ExtractDistanceTrajectory_mex 9 | set MEX_EXT=.mexw64 10 | call setEnv.bat 11 | echo # Make settings for ExtractDistanceTrajectory > ExtractDistanceTrajectory_mex.mki 12 | echo COMPILER=%COMPILER%>> ExtractDistanceTrajectory_mex.mki 13 | echo COMPFLAGS=%COMPFLAGS%>> ExtractDistanceTrajectory_mex.mki 14 | echo OPTIMFLAGS=%OPTIMFLAGS%>> ExtractDistanceTrajectory_mex.mki 15 | echo DEBUGFLAGS=%DEBUGFLAGS%>> ExtractDistanceTrajectory_mex.mki 16 | echo LINKER=%LINKER%>> ExtractDistanceTrajectory_mex.mki 17 | echo LINKFLAGS=%LINKFLAGS%>> ExtractDistanceTrajectory_mex.mki 18 | echo LINKOPTIMFLAGS=%LINKOPTIMFLAGS%>> ExtractDistanceTrajectory_mex.mki 19 | echo LINKDEBUGFLAGS=%LINKDEBUGFLAGS%>> ExtractDistanceTrajectory_mex.mki 20 | echo MATLAB_ARCH=%MATLAB_ARCH%>> ExtractDistanceTrajectory_mex.mki 21 | echo OMPFLAGS= >> ExtractDistanceTrajectory_mex.mki 22 | echo OMPLINKFLAGS= >> ExtractDistanceTrajectory_mex.mki 23 | echo EMC_COMPILER=msvc150>> ExtractDistanceTrajectory_mex.mki 24 | echo EMC_CONFIG=optim>> ExtractDistanceTrajectory_mex.mki 25 | "C:\Program Files\MATLAB\R2018a\bin\win64\gmake" -j 1 -B -f ExtractDistanceTrajectory_mex.mk 26 | -------------------------------------------------------------------------------- /Controllers/MPC/generated/codegen/mex/ExtractDistanceTrajectory/ExtractDistanceTrajectory_mex.exp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mindThomas/Kugle-MATLAB/b1ee16de78899a131241639d620b5fb7c02a586b/Controllers/MPC/generated/codegen/mex/ExtractDistanceTrajectory/ExtractDistanceTrajectory_mex.exp -------------------------------------------------------------------------------- /Controllers/MPC/generated/codegen/mex/ExtractDistanceTrajectory/ExtractDistanceTrajectory_mex.mexw64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mindThomas/Kugle-MATLAB/b1ee16de78899a131241639d620b5fb7c02a586b/Controllers/MPC/generated/codegen/mex/ExtractDistanceTrajectory/ExtractDistanceTrajectory_mex.mexw64 -------------------------------------------------------------------------------- /Controllers/MPC/generated/codegen/mex/ExtractDistanceTrajectory/ExtractDistanceTrajectory_mex.mexw64.manifest: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /Controllers/MPC/generated/codegen/mex/ExtractDistanceTrajectory/ExtractDistanceTrajectory_mex.mki: -------------------------------------------------------------------------------- 1 | # Make settings for ExtractDistanceTrajectory 2 | COMPILER=cl 3 | COMPFLAGS=/c /Zp8 /GR /W3 /EHs /nologo /MD /D_CRT_SECURE_NO_DEPRECATE /D_SCL_SECURE_NO_DEPRECATE /D_SECURE_SCL=0 /DMATLAB_MEX_FILE /DMATLAB_MEX_FILE 4 | OPTIMFLAGS=/O2 /Oy- /DNDEBUG 5 | DEBUGFLAGS=/Z7 6 | LINKER=link 7 | LINKFLAGS=/nologo /manifest /export:mexFunction /DLL /LIBPATH:"C:\Program Files\MATLAB\R2018a\extern\lib\win64\microsoft" libmx.lib libmex.lib libmat.lib kernel32.lib user32.lib gdi32.lib winspool.lib comdlg32.lib advapi32.lib shell32.lib ole32.lib oleaut32.lib uuid.lib odbc32.lib odbccp32.lib /EXPORT:mexFunction 8 | LINKOPTIMFLAGS= 9 | LINKDEBUGFLAGS=/debug /PDB:".\ExtractDistanceTrajectory_mex.mexw64.pdb" 10 | MATLAB_ARCH=win64 11 | OMPFLAGS= 12 | OMPLINKFLAGS= 13 | EMC_COMPILER=msvc150 14 | EMC_CONFIG=optim 15 | -------------------------------------------------------------------------------- /Controllers/MPC/generated/codegen/mex/ExtractDistanceTrajectory/ExtractDistanceTrajectory_mex_mex.arf: -------------------------------------------------------------------------------- 1 | libemlrt.lib 2 | libcovrt.lib 3 | libut.lib 4 | libmwmathutil.lib 5 | /export:mexFunction 6 | /export:mexfilerequiredapiversion 7 | /export:emlrtMexFcnProperties 8 | /export:ExtractDistanceTrajectory 9 | /export:emxEnsureCapacity_int32_T 10 | /export:emxEnsureCapacity_real_T 11 | /export:emxEnsureCapacity_real_T1 12 | /export:emxFree_int32_T 13 | /export:emxFree_real_T 14 | /export:emxInit_int32_T 15 | /export:emxInit_real_T 16 | /export:emxInit_real_T1 17 | /export:ExtractDistanceTrajectory_initialize 18 | /export:ExtractDistanceTrajectory_terminate 19 | /export:ExtractDistanceTrajectory_atexit 20 | -------------------------------------------------------------------------------- /Controllers/MPC/generated/codegen/mex/ExtractDistanceTrajectory/ExtractDistanceTrajectory_terminate.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Academic License - for use in teaching, academic research, and meeting 3 | * course requirements at degree granting institutions only. Not for 4 | * government, commercial, or other organizational use. 5 | * 6 | * ExtractDistanceTrajectory_terminate.h 7 | * 8 | * Code generation for function 'ExtractDistanceTrajectory_terminate' 9 | * 10 | */ 11 | 12 | #ifndef EXTRACTDISTANCETRAJECTORY_TERMINATE_H 13 | #define EXTRACTDISTANCETRAJECTORY_TERMINATE_H 14 | 15 | /* Include files */ 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include "tmwtypes.h" 21 | #include "mex.h" 22 | #include "emlrt.h" 23 | #include "rtwtypes.h" 24 | #include "ExtractDistanceTrajectory_types.h" 25 | 26 | /* Function Declarations */ 27 | extern void ExtractDistanceTrajectory_atexit(void); 28 | extern void ExtractDistanceTrajectory_terminate(void); 29 | 30 | #endif 31 | 32 | /* End of code generation (ExtractDistanceTrajectory_terminate.h) */ 33 | -------------------------------------------------------------------------------- /Controllers/MPC/generated/codegen/mex/ExtractDistanceTrajectory/RotateTrajectory.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Academic License - for use in teaching, academic research, and meeting 3 | * course requirements at degree granting institutions only. Not for 4 | * government, commercial, or other organizational use. 5 | * 6 | * RotateTrajectory.h 7 | * 8 | * Code generation for function 'RotateTrajectory' 9 | * 10 | */ 11 | 12 | #ifndef ROTATETRAJECTORY_H 13 | #define ROTATETRAJECTORY_H 14 | 15 | /* Include files */ 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include "tmwtypes.h" 21 | #include "mex.h" 22 | #include "emlrt.h" 23 | #include "rtwtypes.h" 24 | #include "ExtractDistanceTrajectory_types.h" 25 | 26 | /* Function Declarations */ 27 | extern void RotateTrajectory(const emlrtStack *sp, const emxArray_real_T 28 | *trajectory, real_T rotation, emxArray_real_T *rotatedPoints); 29 | 30 | #endif 31 | 32 | /* End of code generation (RotateTrajectory.h) */ 33 | -------------------------------------------------------------------------------- /Controllers/MPC/generated/codegen/mex/ExtractDistanceTrajectory/buildInfo.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mindThomas/Kugle-MATLAB/b1ee16de78899a131241639d620b5fb7c02a586b/Controllers/MPC/generated/codegen/mex/ExtractDistanceTrajectory/buildInfo.mat -------------------------------------------------------------------------------- /Controllers/MPC/generated/codegen/mex/ExtractDistanceTrajectory/c_mexapi_version.c: -------------------------------------------------------------------------------- 1 | #include 2 | #if !defined(USE_MEX_CMD) || defined(MATLAB_MEXCMD_RELEASE) || defined(MX_COMPAT_32) || defined(MX_COMPAT_64) || TARGET_API_VERSION >= 800 3 | #define MX_TMP_TARGET_API_VER MX_TARGET_API_VER 4 | #else 5 | #define MX_TMP_TARGET_API_VER 0 6 | #endif 7 | MEXFUNCTION_LINKAGE 8 | void mexfilerequiredapiversion(unsigned int* built_by_rel, unsigned int* target_api_ver) 9 | { 10 | *built_by_rel = 0x2018a; 11 | *target_api_ver = MX_TMP_TARGET_API_VER; 12 | } 13 | 14 | -------------------------------------------------------------------------------- /Controllers/MPC/generated/codegen/mex/ExtractDistanceTrajectory/eml_int_forloop_overflow_check.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Academic License - for use in teaching, academic research, and meeting 3 | * course requirements at degree granting institutions only. Not for 4 | * government, commercial, or other organizational use. 5 | * 6 | * eml_int_forloop_overflow_check.c 7 | * 8 | * Code generation for function 'eml_int_forloop_overflow_check' 9 | * 10 | */ 11 | 12 | /* Include files */ 13 | #include "rt_nonfinite.h" 14 | #include "ExtractDistanceTrajectory.h" 15 | #include "eml_int_forloop_overflow_check.h" 16 | 17 | /* Variable Definitions */ 18 | static emlrtRTEInfo ab_emlrtRTEI = { 88,/* lineNo */ 19 | 9, /* colNo */ 20 | "eml_int_forloop_overflow_check", /* fName */ 21 | "C:\\Program Files\\MATLAB\\R2018a\\toolbox\\eml\\lib\\matlab\\eml\\eml_int_forloop_overflow_check.m"/* pName */ 22 | }; 23 | 24 | /* Function Definitions */ 25 | void check_forloop_overflow_error(const emlrtStack *sp) 26 | { 27 | emlrtErrorWithMessageIdR2018a(sp, &ab_emlrtRTEI, 28 | "Coder:toolbox:int_forloop_overflow", "Coder:toolbox:int_forloop_overflow", 29 | 3, 4, 5, "int32"); 30 | } 31 | 32 | /* End of code generation (eml_int_forloop_overflow_check.c) */ 33 | -------------------------------------------------------------------------------- /Controllers/MPC/generated/codegen/mex/ExtractDistanceTrajectory/eml_int_forloop_overflow_check.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Academic License - for use in teaching, academic research, and meeting 3 | * course requirements at degree granting institutions only. Not for 4 | * government, commercial, or other organizational use. 5 | * 6 | * eml_int_forloop_overflow_check.h 7 | * 8 | * Code generation for function 'eml_int_forloop_overflow_check' 9 | * 10 | */ 11 | 12 | #ifndef EML_INT_FORLOOP_OVERFLOW_CHECK_H 13 | #define EML_INT_FORLOOP_OVERFLOW_CHECK_H 14 | 15 | /* Include files */ 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include "tmwtypes.h" 21 | #include "mex.h" 22 | #include "emlrt.h" 23 | #include "rtwtypes.h" 24 | #include "ExtractDistanceTrajectory_types.h" 25 | 26 | /* Function Declarations */ 27 | extern void check_forloop_overflow_error(const emlrtStack *sp); 28 | 29 | #endif 30 | 31 | /* End of code generation (eml_int_forloop_overflow_check.h) */ 32 | -------------------------------------------------------------------------------- /Controllers/MPC/generated/codegen/mex/ExtractDistanceTrajectory/error.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Academic License - for use in teaching, academic research, and meeting 3 | * course requirements at degree granting institutions only. Not for 4 | * government, commercial, or other organizational use. 5 | * 6 | * error.c 7 | * 8 | * Code generation for function 'error' 9 | * 10 | */ 11 | 12 | /* Include files */ 13 | #include "rt_nonfinite.h" 14 | #include "ExtractDistanceTrajectory.h" 15 | #include "error.h" 16 | 17 | /* Variable Definitions */ 18 | static emlrtRTEInfo cb_emlrtRTEI = { 19,/* lineNo */ 19 | 5, /* colNo */ 20 | "error", /* fName */ 21 | "C:\\Program Files\\MATLAB\\R2018a\\toolbox\\shared\\coder\\coder\\+coder\\+internal\\error.m"/* pName */ 22 | }; 23 | 24 | /* Function Definitions */ 25 | void error(const emlrtStack *sp) 26 | { 27 | static const char_T varargin_1[4] = { 's', 'q', 'r', 't' }; 28 | 29 | emlrtErrorWithMessageIdR2018a(sp, &cb_emlrtRTEI, 30 | "Coder:toolbox:ElFunDomainError", "Coder:toolbox:ElFunDomainError", 3, 4, 4, 31 | varargin_1); 32 | } 33 | 34 | /* End of code generation (error.c) */ 35 | -------------------------------------------------------------------------------- /Controllers/MPC/generated/codegen/mex/ExtractDistanceTrajectory/error.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Academic License - for use in teaching, academic research, and meeting 3 | * course requirements at degree granting institutions only. Not for 4 | * government, commercial, or other organizational use. 5 | * 6 | * error.h 7 | * 8 | * Code generation for function 'error' 9 | * 10 | */ 11 | 12 | #ifndef ERROR_H 13 | #define ERROR_H 14 | 15 | /* Include files */ 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include "tmwtypes.h" 21 | #include "mex.h" 22 | #include "emlrt.h" 23 | #include "rtwtypes.h" 24 | #include "ExtractDistanceTrajectory_types.h" 25 | 26 | /* Function Declarations */ 27 | extern void error(const emlrtStack *sp); 28 | 29 | #endif 30 | 31 | /* End of code generation (error.h) */ 32 | -------------------------------------------------------------------------------- /Controllers/MPC/generated/codegen/mex/ExtractDistanceTrajectory/interface/_coder_ExtractDistanceTrajectory_api.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Academic License - for use in teaching, academic research, and meeting 3 | * course requirements at degree granting institutions only. Not for 4 | * government, commercial, or other organizational use. 5 | * 6 | * _coder_ExtractDistanceTrajectory_api.h 7 | * 8 | * Code generation for function '_coder_ExtractDistanceTrajectory_api' 9 | * 10 | */ 11 | 12 | #ifndef _CODER_EXTRACTDISTANCETRAJECTORY_API_H 13 | #define _CODER_EXTRACTDISTANCETRAJECTORY_API_H 14 | 15 | /* Include files */ 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include "tmwtypes.h" 21 | #include "mex.h" 22 | #include "emlrt.h" 23 | #include "rtwtypes.h" 24 | #include "ExtractDistanceTrajectory_types.h" 25 | 26 | /* Function Declarations */ 27 | extern void ExtractDistanceTrajectory_api(const mxArray * const prhs[7], int32_T 28 | nlhs, const mxArray *plhs[4]); 29 | 30 | #endif 31 | 32 | /* End of code generation (_coder_ExtractDistanceTrajectory_api.h) */ 33 | -------------------------------------------------------------------------------- /Controllers/MPC/generated/codegen/mex/ExtractDistanceTrajectory/interface/_coder_ExtractDistanceTrajectory_info.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Academic License - for use in teaching, academic research, and meeting 3 | * course requirements at degree granting institutions only. Not for 4 | * government, commercial, or other organizational use. 5 | * 6 | * _coder_ExtractDistanceTrajectory_info.h 7 | * 8 | * Code generation for function '_coder_ExtractDistanceTrajectory_info' 9 | * 10 | */ 11 | 12 | #ifndef _CODER_EXTRACTDISTANCETRAJECTORY_INFO_H 13 | #define _CODER_EXTRACTDISTANCETRAJECTORY_INFO_H 14 | 15 | /* Include files */ 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include "tmwtypes.h" 21 | #include "mex.h" 22 | #include "emlrt.h" 23 | #include "rtwtypes.h" 24 | #include "ExtractDistanceTrajectory_types.h" 25 | 26 | /* Function Declarations */ 27 | MEXFUNCTION_LINKAGE mxArray *emlrtMexFcnProperties(void); 28 | extern const mxArray *emlrtMexFcnResolvedFunctionsInfo(void); 29 | 30 | #endif 31 | 32 | /* End of code generation (_coder_ExtractDistanceTrajectory_info.h) */ 33 | -------------------------------------------------------------------------------- /Controllers/MPC/generated/codegen/mex/ExtractDistanceTrajectory/interface/_coder_ExtractDistanceTrajectory_mex.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Academic License - for use in teaching, academic research, and meeting 3 | * course requirements at degree granting institutions only. Not for 4 | * government, commercial, or other organizational use. 5 | * 6 | * _coder_ExtractDistanceTrajectory_mex.h 7 | * 8 | * Code generation for function '_coder_ExtractDistanceTrajectory_mex' 9 | * 10 | */ 11 | 12 | #ifndef _CODER_EXTRACTDISTANCETRAJECTORY_MEX_H 13 | #define _CODER_EXTRACTDISTANCETRAJECTORY_MEX_H 14 | 15 | /* Include files */ 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include "tmwtypes.h" 21 | #include "mex.h" 22 | #include "emlrt.h" 23 | #include "rtwtypes.h" 24 | #include "ExtractDistanceTrajectory_types.h" 25 | 26 | /* Function Declarations */ 27 | extern void mexFunction(int32_T nlhs, mxArray *plhs[], int32_T nrhs, const 28 | mxArray *prhs[]); 29 | extern emlrtCTX mexFunctionCreateRootTLS(void); 30 | 31 | #endif 32 | 33 | /* End of code generation (_coder_ExtractDistanceTrajectory_mex.h) */ 34 | -------------------------------------------------------------------------------- /Controllers/MPC/generated/codegen/mex/ExtractDistanceTrajectory/mod.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Academic License - for use in teaching, academic research, and meeting 3 | * course requirements at degree granting institutions only. Not for 4 | * government, commercial, or other organizational use. 5 | * 6 | * mod.c 7 | * 8 | * Code generation for function 'mod' 9 | * 10 | */ 11 | 12 | /* Include files */ 13 | #include "mwmathutil.h" 14 | #include "rt_nonfinite.h" 15 | #include "ExtractDistanceTrajectory.h" 16 | #include "mod.h" 17 | 18 | /* Function Definitions */ 19 | real_T b_mod(real_T x, real_T y) 20 | { 21 | real_T r; 22 | r = x; 23 | if ((!muDoubleScalarIsInf(x)) && (!muDoubleScalarIsNaN(x)) && 24 | ((!muDoubleScalarIsInf(y)) && (!muDoubleScalarIsNaN(y)))) { 25 | if (x == 0.0) { 26 | r = 0.0; 27 | } else { 28 | if (y != 0.0) { 29 | r = muDoubleScalarRem(x, y); 30 | if (r == 0.0) { 31 | r = 0.0; 32 | } else { 33 | if (x < 0.0) { 34 | r += y; 35 | } 36 | } 37 | } 38 | } 39 | } else { 40 | if (y != 0.0) { 41 | r = rtNaN; 42 | } 43 | } 44 | 45 | return r; 46 | } 47 | 48 | /* End of code generation (mod.c) */ 49 | -------------------------------------------------------------------------------- /Controllers/MPC/generated/codegen/mex/ExtractDistanceTrajectory/mod.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Academic License - for use in teaching, academic research, and meeting 3 | * course requirements at degree granting institutions only. Not for 4 | * government, commercial, or other organizational use. 5 | * 6 | * mod.h 7 | * 8 | * Code generation for function 'mod' 9 | * 10 | */ 11 | 12 | #ifndef MOD_H 13 | #define MOD_H 14 | 15 | /* Include files */ 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include "tmwtypes.h" 21 | #include "mex.h" 22 | #include "emlrt.h" 23 | #include "rtwtypes.h" 24 | #include "ExtractDistanceTrajectory_types.h" 25 | 26 | /* Function Declarations */ 27 | extern real_T b_mod(real_T x, real_T y); 28 | 29 | #endif 30 | 31 | /* End of code generation (mod.h) */ 32 | -------------------------------------------------------------------------------- /Controllers/MPC/generated/codegen/mex/ExtractDistanceTrajectory/power.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Academic License - for use in teaching, academic research, and meeting 3 | * course requirements at degree granting institutions only. Not for 4 | * government, commercial, or other organizational use. 5 | * 6 | * power.h 7 | * 8 | * Code generation for function 'power' 9 | * 10 | */ 11 | 12 | #ifndef POWER_H 13 | #define POWER_H 14 | 15 | /* Include files */ 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include "tmwtypes.h" 21 | #include "mex.h" 22 | #include "emlrt.h" 23 | #include "rtwtypes.h" 24 | #include "ExtractDistanceTrajectory_types.h" 25 | 26 | /* Function Declarations */ 27 | extern void power(const emlrtStack *sp, const emxArray_real_T *a, 28 | emxArray_real_T *y); 29 | 30 | #endif 31 | 32 | /* End of code generation (power.h) */ 33 | -------------------------------------------------------------------------------- /Controllers/MPC/generated/codegen/mex/ExtractDistanceTrajectory/repmat.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Academic License - for use in teaching, academic research, and meeting 3 | * course requirements at degree granting institutions only. Not for 4 | * government, commercial, or other organizational use. 5 | * 6 | * repmat.h 7 | * 8 | * Code generation for function 'repmat' 9 | * 10 | */ 11 | 12 | #ifndef REPMAT_H 13 | #define REPMAT_H 14 | 15 | /* Include files */ 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include "tmwtypes.h" 21 | #include "mex.h" 22 | #include "emlrt.h" 23 | #include "rtwtypes.h" 24 | #include "ExtractDistanceTrajectory_types.h" 25 | 26 | /* Function Declarations */ 27 | extern void repmat(const emlrtStack *sp, const real_T a[2], const real_T 28 | varargin_1[2], emxArray_real_T *b); 29 | 30 | #endif 31 | 32 | /* End of code generation (repmat.h) */ 33 | -------------------------------------------------------------------------------- /Controllers/MPC/generated/codegen/mex/ExtractDistanceTrajectory/rt_nonfinite.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Academic License - for use in teaching, academic research, and meeting 3 | * course requirements at degree granting institutions only. Not for 4 | * government, commercial, or other organizational use. 5 | * 6 | * rt_nonfinite.h 7 | * 8 | * Code generation for function 'ExtractDistanceTrajectory' 9 | * 10 | */ 11 | 12 | #ifndef RT_NONFINITE_H 13 | #define RT_NONFINITE_H 14 | #define rtInf mxGetInf() 15 | #define rtMinusInf (-mxGetInf()) 16 | #define rtNaN mxGetNaN() 17 | #define rtIsNaN(X) mxIsNaN(X) 18 | #define rtIsInf(X) mxIsInf(X) 19 | #define rtIsNaNF(X) mxIsNaN(X) 20 | #define rtIsInfF(X) mxIsInf(X) 21 | #endif 22 | /* End of code generation (rt_nonfinite.h) */ 23 | -------------------------------------------------------------------------------- /Controllers/MPC/generated/codegen/mex/ExtractDistanceTrajectory/rtwtypes.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Academic License - for use in teaching, academic research, and meeting 3 | * course requirements at degree granting institutions only. Not for 4 | * government, commercial, or other organizational use. 5 | * 6 | * rtwtypes.h 7 | * 8 | * Code generation for function 'ExtractDistanceTrajectory' 9 | * 10 | */ 11 | 12 | #ifndef RTWTYPES_H 13 | #define RTWTYPES_H 14 | #include "tmwtypes.h" 15 | /* 16 | * TRUE/FALSE definitions 17 | */ 18 | #ifndef TRUE 19 | #define TRUE (1U) 20 | #endif 21 | #ifndef FALSE 22 | #define FALSE (0U) 23 | #endif 24 | #endif 25 | /* End of code generation (rtwtypes.h) */ 26 | -------------------------------------------------------------------------------- /Controllers/MPC/generated/codegen/mex/ExtractDistanceTrajectory/scalexpAlloc.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Academic License - for use in teaching, academic research, and meeting 3 | * course requirements at degree granting institutions only. Not for 4 | * government, commercial, or other organizational use. 5 | * 6 | * scalexpAlloc.c 7 | * 8 | * Code generation for function 'scalexpAlloc' 9 | * 10 | */ 11 | 12 | /* Include files */ 13 | #include "rt_nonfinite.h" 14 | #include "ExtractDistanceTrajectory.h" 15 | #include "scalexpAlloc.h" 16 | 17 | /* Function Definitions */ 18 | boolean_T dimagree(const emxArray_real_T *z, const emxArray_real_T *varargin_1) 19 | { 20 | boolean_T p; 21 | boolean_T b_p; 22 | int32_T k; 23 | boolean_T exitg1; 24 | int32_T i3; 25 | int32_T i4; 26 | p = true; 27 | b_p = true; 28 | k = 1; 29 | exitg1 = false; 30 | while ((!exitg1) && (k < 3)) { 31 | if (k <= 1) { 32 | i3 = z->size[0]; 33 | i4 = varargin_1->size[0]; 34 | } else { 35 | i3 = 1; 36 | i4 = 1; 37 | } 38 | 39 | if (i3 != i4) { 40 | b_p = false; 41 | exitg1 = true; 42 | } else { 43 | k++; 44 | } 45 | } 46 | 47 | if (!b_p) { 48 | p = false; 49 | } 50 | 51 | return p; 52 | } 53 | 54 | /* End of code generation (scalexpAlloc.c) */ 55 | -------------------------------------------------------------------------------- /Controllers/MPC/generated/codegen/mex/ExtractDistanceTrajectory/scalexpAlloc.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Academic License - for use in teaching, academic research, and meeting 3 | * course requirements at degree granting institutions only. Not for 4 | * government, commercial, or other organizational use. 5 | * 6 | * scalexpAlloc.h 7 | * 8 | * Code generation for function 'scalexpAlloc' 9 | * 10 | */ 11 | 12 | #ifndef SCALEXPALLOC_H 13 | #define SCALEXPALLOC_H 14 | 15 | /* Include files */ 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include "tmwtypes.h" 21 | #include "mex.h" 22 | #include "emlrt.h" 23 | #include "rtwtypes.h" 24 | #include "ExtractDistanceTrajectory_types.h" 25 | 26 | /* Function Declarations */ 27 | extern boolean_T dimagree(const emxArray_real_T *z, const emxArray_real_T 28 | *varargin_1); 29 | 30 | #endif 31 | 32 | /* End of code generation (scalexpAlloc.h) */ 33 | -------------------------------------------------------------------------------- /Controllers/MPC/generated/codegen/mex/ExtractDistanceTrajectory/sort1.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Academic License - for use in teaching, academic research, and meeting 3 | * course requirements at degree granting institutions only. Not for 4 | * government, commercial, or other organizational use. 5 | * 6 | * sort1.h 7 | * 8 | * Code generation for function 'sort1' 9 | * 10 | */ 11 | 12 | #ifndef SORT1_H 13 | #define SORT1_H 14 | 15 | /* Include files */ 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include "tmwtypes.h" 21 | #include "mex.h" 22 | #include "emlrt.h" 23 | #include "rtwtypes.h" 24 | #include "ExtractDistanceTrajectory_types.h" 25 | 26 | /* Function Declarations */ 27 | extern void sort(const emlrtStack *sp, emxArray_real_T *x, emxArray_int32_T *idx); 28 | 29 | #endif 30 | 31 | /* End of code generation (sort1.h) */ 32 | -------------------------------------------------------------------------------- /Controllers/MPC/generated/codegen/mex/ExtractDistanceTrajectory/sortIdx.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Academic License - for use in teaching, academic research, and meeting 3 | * course requirements at degree granting institutions only. Not for 4 | * government, commercial, or other organizational use. 5 | * 6 | * sortIdx.h 7 | * 8 | * Code generation for function 'sortIdx' 9 | * 10 | */ 11 | 12 | #ifndef SORTIDX_H 13 | #define SORTIDX_H 14 | 15 | /* Include files */ 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include "tmwtypes.h" 21 | #include "mex.h" 22 | #include "emlrt.h" 23 | #include "rtwtypes.h" 24 | #include "ExtractDistanceTrajectory_types.h" 25 | 26 | /* Function Declarations */ 27 | extern void sortIdx(const emlrtStack *sp, emxArray_real_T *x, emxArray_int32_T 28 | *idx); 29 | 30 | #endif 31 | 32 | /* End of code generation (sortIdx.h) */ 33 | -------------------------------------------------------------------------------- /Controllers/MPC/generated/codegen/mex/ExtractWindowTrajectory/ExtractWindowTrajectory.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Academic License - for use in teaching, academic research, and meeting 3 | * course requirements at degree granting institutions only. Not for 4 | * government, commercial, or other organizational use. 5 | * 6 | * ExtractWindowTrajectory.h 7 | * 8 | * Code generation for function 'ExtractWindowTrajectory' 9 | * 10 | */ 11 | 12 | #ifndef EXTRACTWINDOWTRAJECTORY_H 13 | #define EXTRACTWINDOWTRAJECTORY_H 14 | 15 | /* Include files */ 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include "tmwtypes.h" 21 | #include "mex.h" 22 | #include "emlrt.h" 23 | #include "rtwtypes.h" 24 | #include "ExtractWindowTrajectory_types.h" 25 | 26 | /* Function Declarations */ 27 | extern void ExtractWindowTrajectory(const emlrtStack *sp, const emxArray_real_T * 28 | TrajectoryPoints, const real_T RobotPos[2], real_T RobotYaw, const real_T 29 | Velocity[2], real_T ExtractDist, real_T WindowWidth, real_T WindowHeight, 30 | const real_T WindowOffset[2], real_T OrientationSelection, emxArray_real_T 31 | *WindowTrajectory, real_T *nTrajPoints, real_T *WindowOrientation); 32 | 33 | #endif 34 | 35 | /* End of code generation (ExtractWindowTrajectory.h) */ 36 | -------------------------------------------------------------------------------- /Controllers/MPC/generated/codegen/mex/ExtractWindowTrajectory/ExtractWindowTrajectory_data.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Academic License - for use in teaching, academic research, and meeting 3 | * course requirements at degree granting institutions only. Not for 4 | * government, commercial, or other organizational use. 5 | * 6 | * ExtractWindowTrajectory_data.h 7 | * 8 | * Code generation for function 'ExtractWindowTrajectory_data' 9 | * 10 | */ 11 | 12 | #ifndef EXTRACTWINDOWTRAJECTORY_DATA_H 13 | #define EXTRACTWINDOWTRAJECTORY_DATA_H 14 | 15 | /* Include files */ 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include "tmwtypes.h" 21 | #include "mex.h" 22 | #include "emlrt.h" 23 | #include "rtwtypes.h" 24 | #include "ExtractWindowTrajectory_types.h" 25 | 26 | /* Variable Declarations */ 27 | extern emlrtCTX emlrtRootTLSGlobal; 28 | extern emlrtContext emlrtContextGlobal; 29 | extern emlrtRSInfo x_emlrtRSI; 30 | extern emlrtRSInfo bb_emlrtRSI; 31 | extern emlrtRSInfo ic_emlrtRSI; 32 | extern emlrtRSInfo wc_emlrtRSI; 33 | 34 | #endif 35 | 36 | /* End of code generation (ExtractWindowTrajectory_data.h) */ 37 | -------------------------------------------------------------------------------- /Controllers/MPC/generated/codegen/mex/ExtractWindowTrajectory/ExtractWindowTrajectory_initialize.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Academic License - for use in teaching, academic research, and meeting 3 | * course requirements at degree granting institutions only. Not for 4 | * government, commercial, or other organizational use. 5 | * 6 | * ExtractWindowTrajectory_initialize.c 7 | * 8 | * Code generation for function 'ExtractWindowTrajectory_initialize' 9 | * 10 | */ 11 | 12 | /* Include files */ 13 | #include "rt_nonfinite.h" 14 | #include "ExtractWindowTrajectory.h" 15 | #include "ExtractWindowTrajectory_initialize.h" 16 | #include "_coder_ExtractWindowTrajectory_mex.h" 17 | #include "ExtractWindowTrajectory_data.h" 18 | 19 | /* Variable Definitions */ 20 | static const volatile char_T *emlrtBreakCheckR2012bFlagVar = NULL; 21 | 22 | /* Function Definitions */ 23 | void ExtractWindowTrajectory_initialize(void) 24 | { 25 | emlrtStack st = { NULL, /* site */ 26 | NULL, /* tls */ 27 | NULL /* prev */ 28 | }; 29 | 30 | mexFunctionCreateRootTLS(); 31 | emlrtBreakCheckR2012bFlagVar = emlrtGetBreakCheckFlagAddressR2012b(); 32 | st.tls = emlrtRootTLSGlobal; 33 | emlrtClearAllocCountR2012b(&st, false, 0U, 0); 34 | emlrtEnterRtStackR2012b(&st); 35 | emlrtFirstTimeR2012b(emlrtRootTLSGlobal); 36 | } 37 | 38 | /* End of code generation (ExtractWindowTrajectory_initialize.c) */ 39 | -------------------------------------------------------------------------------- /Controllers/MPC/generated/codegen/mex/ExtractWindowTrajectory/ExtractWindowTrajectory_initialize.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Academic License - for use in teaching, academic research, and meeting 3 | * course requirements at degree granting institutions only. Not for 4 | * government, commercial, or other organizational use. 5 | * 6 | * ExtractWindowTrajectory_initialize.h 7 | * 8 | * Code generation for function 'ExtractWindowTrajectory_initialize' 9 | * 10 | */ 11 | 12 | #ifndef EXTRACTWINDOWTRAJECTORY_INITIALIZE_H 13 | #define EXTRACTWINDOWTRAJECTORY_INITIALIZE_H 14 | 15 | /* Include files */ 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include "tmwtypes.h" 21 | #include "mex.h" 22 | #include "emlrt.h" 23 | #include "rtwtypes.h" 24 | #include "ExtractWindowTrajectory_types.h" 25 | 26 | /* Function Declarations */ 27 | extern void ExtractWindowTrajectory_initialize(void); 28 | 29 | #endif 30 | 31 | /* End of code generation (ExtractWindowTrajectory_initialize.h) */ 32 | -------------------------------------------------------------------------------- /Controllers/MPC/generated/codegen/mex/ExtractWindowTrajectory/ExtractWindowTrajectory_mex.bat: -------------------------------------------------------------------------------- 1 | @echo off 2 | set MATLAB=C:\PROGRA~1\MATLAB\R2018a 3 | set MATLAB_ARCH=win64 4 | set MATLAB_BIN="C:\Program Files\MATLAB\R2018a\bin" 5 | set ENTRYPOINT=mexFunction 6 | set OUTDIR=.\ 7 | set LIB_NAME=ExtractWindowTrajectory_mex 8 | set MEX_NAME=ExtractWindowTrajectory_mex 9 | set MEX_EXT=.mexw64 10 | call setEnv.bat 11 | echo # Make settings for ExtractWindowTrajectory > ExtractWindowTrajectory_mex.mki 12 | echo COMPILER=%COMPILER%>> ExtractWindowTrajectory_mex.mki 13 | echo COMPFLAGS=%COMPFLAGS%>> ExtractWindowTrajectory_mex.mki 14 | echo OPTIMFLAGS=%OPTIMFLAGS%>> ExtractWindowTrajectory_mex.mki 15 | echo DEBUGFLAGS=%DEBUGFLAGS%>> ExtractWindowTrajectory_mex.mki 16 | echo LINKER=%LINKER%>> ExtractWindowTrajectory_mex.mki 17 | echo LINKFLAGS=%LINKFLAGS%>> ExtractWindowTrajectory_mex.mki 18 | echo LINKOPTIMFLAGS=%LINKOPTIMFLAGS%>> ExtractWindowTrajectory_mex.mki 19 | echo LINKDEBUGFLAGS=%LINKDEBUGFLAGS%>> ExtractWindowTrajectory_mex.mki 20 | echo MATLAB_ARCH=%MATLAB_ARCH%>> ExtractWindowTrajectory_mex.mki 21 | echo OMPFLAGS= >> ExtractWindowTrajectory_mex.mki 22 | echo OMPLINKFLAGS= >> ExtractWindowTrajectory_mex.mki 23 | echo EMC_COMPILER=msvc150>> ExtractWindowTrajectory_mex.mki 24 | echo EMC_CONFIG=optim>> ExtractWindowTrajectory_mex.mki 25 | "C:\Program Files\MATLAB\R2018a\bin\win64\gmake" -j 1 -B -f ExtractWindowTrajectory_mex.mk 26 | -------------------------------------------------------------------------------- /Controllers/MPC/generated/codegen/mex/ExtractWindowTrajectory/ExtractWindowTrajectory_mex.exp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mindThomas/Kugle-MATLAB/b1ee16de78899a131241639d620b5fb7c02a586b/Controllers/MPC/generated/codegen/mex/ExtractWindowTrajectory/ExtractWindowTrajectory_mex.exp -------------------------------------------------------------------------------- /Controllers/MPC/generated/codegen/mex/ExtractWindowTrajectory/ExtractWindowTrajectory_mex.mexw64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mindThomas/Kugle-MATLAB/b1ee16de78899a131241639d620b5fb7c02a586b/Controllers/MPC/generated/codegen/mex/ExtractWindowTrajectory/ExtractWindowTrajectory_mex.mexw64 -------------------------------------------------------------------------------- /Controllers/MPC/generated/codegen/mex/ExtractWindowTrajectory/ExtractWindowTrajectory_mex.mexw64.manifest: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /Controllers/MPC/generated/codegen/mex/ExtractWindowTrajectory/ExtractWindowTrajectory_mex.mki: -------------------------------------------------------------------------------- 1 | # Make settings for ExtractWindowTrajectory 2 | COMPILER=cl 3 | COMPFLAGS=/c /Zp8 /GR /W3 /EHs /nologo /MD /D_CRT_SECURE_NO_DEPRECATE /D_SCL_SECURE_NO_DEPRECATE /D_SECURE_SCL=0 /DMATLAB_MEX_FILE /DMATLAB_MEX_FILE 4 | OPTIMFLAGS=/O2 /Oy- /DNDEBUG 5 | DEBUGFLAGS=/Z7 6 | LINKER=link 7 | LINKFLAGS=/nologo /manifest /export:mexFunction /DLL /LIBPATH:"C:\Program Files\MATLAB\R2018a\extern\lib\win64\microsoft" libmx.lib libmex.lib libmat.lib kernel32.lib user32.lib gdi32.lib winspool.lib comdlg32.lib advapi32.lib shell32.lib ole32.lib oleaut32.lib uuid.lib odbc32.lib odbccp32.lib /EXPORT:mexFunction 8 | LINKOPTIMFLAGS= 9 | LINKDEBUGFLAGS=/debug /PDB:".\ExtractWindowTrajectory_mex.mexw64.pdb" 10 | MATLAB_ARCH=win64 11 | OMPFLAGS= 12 | OMPLINKFLAGS= 13 | EMC_COMPILER=msvc150 14 | EMC_CONFIG=optim 15 | -------------------------------------------------------------------------------- /Controllers/MPC/generated/codegen/mex/ExtractWindowTrajectory/ExtractWindowTrajectory_mex_mex.arf: -------------------------------------------------------------------------------- 1 | libemlrt.lib 2 | libcovrt.lib 3 | libut.lib 4 | libmwmathutil.lib 5 | /export:mexFunction 6 | /export:mexfilerequiredapiversion 7 | /export:emlrtMexFcnProperties 8 | /export:ExtractWindowTrajectory 9 | /export:emxEnsureCapacity_boolean_T 10 | /export:emxEnsureCapacity_int32_T 11 | /export:emxEnsureCapacity_real_T 12 | /export:emxEnsureCapacity_real_T1 13 | /export:emxFree_boolean_T 14 | /export:emxFree_int32_T 15 | /export:emxFree_real_T 16 | /export:emxInit_boolean_T 17 | /export:emxInit_int32_T 18 | /export:emxInit_real_T 19 | /export:emxInit_real_T1 20 | /export:ExtractWindowTrajectory_initialize 21 | /export:ExtractWindowTrajectory_terminate 22 | /export:ExtractWindowTrajectory_atexit 23 | -------------------------------------------------------------------------------- /Controllers/MPC/generated/codegen/mex/ExtractWindowTrajectory/ExtractWindowTrajectory_terminate.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Academic License - for use in teaching, academic research, and meeting 3 | * course requirements at degree granting institutions only. Not for 4 | * government, commercial, or other organizational use. 5 | * 6 | * ExtractWindowTrajectory_terminate.h 7 | * 8 | * Code generation for function 'ExtractWindowTrajectory_terminate' 9 | * 10 | */ 11 | 12 | #ifndef EXTRACTWINDOWTRAJECTORY_TERMINATE_H 13 | #define EXTRACTWINDOWTRAJECTORY_TERMINATE_H 14 | 15 | /* Include files */ 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include "tmwtypes.h" 21 | #include "mex.h" 22 | #include "emlrt.h" 23 | #include "rtwtypes.h" 24 | #include "ExtractWindowTrajectory_types.h" 25 | 26 | /* Function Declarations */ 27 | extern void ExtractWindowTrajectory_atexit(void); 28 | extern void ExtractWindowTrajectory_terminate(void); 29 | 30 | #endif 31 | 32 | /* End of code generation (ExtractWindowTrajectory_terminate.h) */ 33 | -------------------------------------------------------------------------------- /Controllers/MPC/generated/codegen/mex/ExtractWindowTrajectory/RotateTrajectory.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Academic License - for use in teaching, academic research, and meeting 3 | * course requirements at degree granting institutions only. Not for 4 | * government, commercial, or other organizational use. 5 | * 6 | * RotateTrajectory.h 7 | * 8 | * Code generation for function 'RotateTrajectory' 9 | * 10 | */ 11 | 12 | #ifndef ROTATETRAJECTORY_H 13 | #define ROTATETRAJECTORY_H 14 | 15 | /* Include files */ 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include "tmwtypes.h" 21 | #include "mex.h" 22 | #include "emlrt.h" 23 | #include "rtwtypes.h" 24 | #include "ExtractWindowTrajectory_types.h" 25 | 26 | /* Function Declarations */ 27 | extern void RotateTrajectory(const emlrtStack *sp, const emxArray_real_T 28 | *trajectory, real_T rotation, emxArray_real_T *rotatedPoints); 29 | 30 | #endif 31 | 32 | /* End of code generation (RotateTrajectory.h) */ 33 | -------------------------------------------------------------------------------- /Controllers/MPC/generated/codegen/mex/ExtractWindowTrajectory/abs.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Academic License - for use in teaching, academic research, and meeting 3 | * course requirements at degree granting institutions only. Not for 4 | * government, commercial, or other organizational use. 5 | * 6 | * abs.h 7 | * 8 | * Code generation for function 'abs' 9 | * 10 | */ 11 | 12 | #ifndef ABS_H 13 | #define ABS_H 14 | 15 | /* Include files */ 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include "tmwtypes.h" 21 | #include "mex.h" 22 | #include "emlrt.h" 23 | #include "rtwtypes.h" 24 | #include "ExtractWindowTrajectory_types.h" 25 | 26 | /* Function Declarations */ 27 | extern void b_abs(const emlrtStack *sp, const emxArray_real_T *x, 28 | emxArray_real_T *y); 29 | 30 | #endif 31 | 32 | /* End of code generation (abs.h) */ 33 | -------------------------------------------------------------------------------- /Controllers/MPC/generated/codegen/mex/ExtractWindowTrajectory/assertValidSizeArg.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Academic License - for use in teaching, academic research, and meeting 3 | * course requirements at degree granting institutions only. Not for 4 | * government, commercial, or other organizational use. 5 | * 6 | * assertValidSizeArg.h 7 | * 8 | * Code generation for function 'assertValidSizeArg' 9 | * 10 | */ 11 | 12 | #ifndef ASSERTVALIDSIZEARG_H 13 | #define ASSERTVALIDSIZEARG_H 14 | 15 | /* Include files */ 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include "tmwtypes.h" 21 | #include "mex.h" 22 | #include "emlrt.h" 23 | #include "rtwtypes.h" 24 | #include "ExtractWindowTrajectory_types.h" 25 | 26 | /* Function Declarations */ 27 | extern void assertValidSizeArg(const emlrtStack *sp, const real_T varargin_1[2]); 28 | 29 | #endif 30 | 31 | /* End of code generation (assertValidSizeArg.h) */ 32 | -------------------------------------------------------------------------------- /Controllers/MPC/generated/codegen/mex/ExtractWindowTrajectory/buildInfo.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mindThomas/Kugle-MATLAB/b1ee16de78899a131241639d620b5fb7c02a586b/Controllers/MPC/generated/codegen/mex/ExtractWindowTrajectory/buildInfo.mat -------------------------------------------------------------------------------- /Controllers/MPC/generated/codegen/mex/ExtractWindowTrajectory/c_mexapi_version.c: -------------------------------------------------------------------------------- 1 | #include 2 | #if !defined(USE_MEX_CMD) || defined(MATLAB_MEXCMD_RELEASE) || defined(MX_COMPAT_32) || defined(MX_COMPAT_64) || TARGET_API_VERSION >= 800 3 | #define MX_TMP_TARGET_API_VER MX_TARGET_API_VER 4 | #else 5 | #define MX_TMP_TARGET_API_VER 0 6 | #endif 7 | MEXFUNCTION_LINKAGE 8 | void mexfilerequiredapiversion(unsigned int* built_by_rel, unsigned int* target_api_ver) 9 | { 10 | *built_by_rel = 0x2018a; 11 | *target_api_ver = MX_TMP_TARGET_API_VER; 12 | } 13 | 14 | -------------------------------------------------------------------------------- /Controllers/MPC/generated/codegen/mex/ExtractWindowTrajectory/diff.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Academic License - for use in teaching, academic research, and meeting 3 | * course requirements at degree granting institutions only. Not for 4 | * government, commercial, or other organizational use. 5 | * 6 | * diff.h 7 | * 8 | * Code generation for function 'diff' 9 | * 10 | */ 11 | 12 | #ifndef DIFF_H 13 | #define DIFF_H 14 | 15 | /* Include files */ 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include "tmwtypes.h" 21 | #include "mex.h" 22 | #include "emlrt.h" 23 | #include "rtwtypes.h" 24 | #include "ExtractWindowTrajectory_types.h" 25 | 26 | /* Function Declarations */ 27 | extern void b_diff(const emlrtStack *sp, const emxArray_real_T *x, 28 | emxArray_real_T *y); 29 | extern void diff(const emlrtStack *sp, const emxArray_real_T *x, emxArray_real_T 30 | *y); 31 | 32 | #endif 33 | 34 | /* End of code generation (diff.h) */ 35 | -------------------------------------------------------------------------------- /Controllers/MPC/generated/codegen/mex/ExtractWindowTrajectory/eml_int_forloop_overflow_check.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Academic License - for use in teaching, academic research, and meeting 3 | * course requirements at degree granting institutions only. Not for 4 | * government, commercial, or other organizational use. 5 | * 6 | * eml_int_forloop_overflow_check.c 7 | * 8 | * Code generation for function 'eml_int_forloop_overflow_check' 9 | * 10 | */ 11 | 12 | /* Include files */ 13 | #include "rt_nonfinite.h" 14 | #include "ExtractWindowTrajectory.h" 15 | #include "eml_int_forloop_overflow_check.h" 16 | 17 | /* Variable Definitions */ 18 | static emlrtRTEInfo hb_emlrtRTEI = { 88,/* lineNo */ 19 | 9, /* colNo */ 20 | "eml_int_forloop_overflow_check", /* fName */ 21 | "C:\\Program Files\\MATLAB\\R2018a\\toolbox\\eml\\lib\\matlab\\eml\\eml_int_forloop_overflow_check.m"/* pName */ 22 | }; 23 | 24 | /* Function Definitions */ 25 | void check_forloop_overflow_error(const emlrtStack *sp) 26 | { 27 | emlrtErrorWithMessageIdR2018a(sp, &hb_emlrtRTEI, 28 | "Coder:toolbox:int_forloop_overflow", "Coder:toolbox:int_forloop_overflow", 29 | 3, 4, 5, "int32"); 30 | } 31 | 32 | /* End of code generation (eml_int_forloop_overflow_check.c) */ 33 | -------------------------------------------------------------------------------- /Controllers/MPC/generated/codegen/mex/ExtractWindowTrajectory/eml_int_forloop_overflow_check.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Academic License - for use in teaching, academic research, and meeting 3 | * course requirements at degree granting institutions only. Not for 4 | * government, commercial, or other organizational use. 5 | * 6 | * eml_int_forloop_overflow_check.h 7 | * 8 | * Code generation for function 'eml_int_forloop_overflow_check' 9 | * 10 | */ 11 | 12 | #ifndef EML_INT_FORLOOP_OVERFLOW_CHECK_H 13 | #define EML_INT_FORLOOP_OVERFLOW_CHECK_H 14 | 15 | /* Include files */ 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include "tmwtypes.h" 21 | #include "mex.h" 22 | #include "emlrt.h" 23 | #include "rtwtypes.h" 24 | #include "ExtractWindowTrajectory_types.h" 25 | 26 | /* Function Declarations */ 27 | extern void check_forloop_overflow_error(const emlrtStack *sp); 28 | 29 | #endif 30 | 31 | /* End of code generation (eml_int_forloop_overflow_check.h) */ 32 | -------------------------------------------------------------------------------- /Controllers/MPC/generated/codegen/mex/ExtractWindowTrajectory/error.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Academic License - for use in teaching, academic research, and meeting 3 | * course requirements at degree granting institutions only. Not for 4 | * government, commercial, or other organizational use. 5 | * 6 | * error.c 7 | * 8 | * Code generation for function 'error' 9 | * 10 | */ 11 | 12 | /* Include files */ 13 | #include "rt_nonfinite.h" 14 | #include "ExtractWindowTrajectory.h" 15 | #include "error.h" 16 | 17 | /* Variable Definitions */ 18 | static emlrtRTEInfo lb_emlrtRTEI = { 19,/* lineNo */ 19 | 5, /* colNo */ 20 | "error", /* fName */ 21 | "C:\\Program Files\\MATLAB\\R2018a\\toolbox\\shared\\coder\\coder\\+coder\\+internal\\error.m"/* pName */ 22 | }; 23 | 24 | /* Function Definitions */ 25 | void error(const emlrtStack *sp) 26 | { 27 | static const char_T varargin_1[4] = { 's', 'q', 'r', 't' }; 28 | 29 | emlrtErrorWithMessageIdR2018a(sp, &lb_emlrtRTEI, 30 | "Coder:toolbox:ElFunDomainError", "Coder:toolbox:ElFunDomainError", 3, 4, 4, 31 | varargin_1); 32 | } 33 | 34 | /* End of code generation (error.c) */ 35 | -------------------------------------------------------------------------------- /Controllers/MPC/generated/codegen/mex/ExtractWindowTrajectory/error.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Academic License - for use in teaching, academic research, and meeting 3 | * course requirements at degree granting institutions only. Not for 4 | * government, commercial, or other organizational use. 5 | * 6 | * error.h 7 | * 8 | * Code generation for function 'error' 9 | * 10 | */ 11 | 12 | #ifndef ERROR_H 13 | #define ERROR_H 14 | 15 | /* Include files */ 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include "tmwtypes.h" 21 | #include "mex.h" 22 | #include "emlrt.h" 23 | #include "rtwtypes.h" 24 | #include "ExtractWindowTrajectory_types.h" 25 | 26 | /* Function Declarations */ 27 | extern void error(const emlrtStack *sp); 28 | 29 | #endif 30 | 31 | /* End of code generation (error.h) */ 32 | -------------------------------------------------------------------------------- /Controllers/MPC/generated/codegen/mex/ExtractWindowTrajectory/ifWhileCond.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Academic License - for use in teaching, academic research, and meeting 3 | * course requirements at degree granting institutions only. Not for 4 | * government, commercial, or other organizational use. 5 | * 6 | * ifWhileCond.h 7 | * 8 | * Code generation for function 'ifWhileCond' 9 | * 10 | */ 11 | 12 | #ifndef IFWHILECOND_H 13 | #define IFWHILECOND_H 14 | 15 | /* Include files */ 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include "tmwtypes.h" 21 | #include "mex.h" 22 | #include "emlrt.h" 23 | #include "rtwtypes.h" 24 | #include "ExtractWindowTrajectory_types.h" 25 | 26 | /* Function Declarations */ 27 | extern boolean_T ifWhileCond(const emlrtStack *sp, const emxArray_boolean_T *x); 28 | 29 | #endif 30 | 31 | /* End of code generation (ifWhileCond.h) */ 32 | -------------------------------------------------------------------------------- /Controllers/MPC/generated/codegen/mex/ExtractWindowTrajectory/indexShapeCheck.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Academic License - for use in teaching, academic research, and meeting 3 | * course requirements at degree granting institutions only. Not for 4 | * government, commercial, or other organizational use. 5 | * 6 | * indexShapeCheck.h 7 | * 8 | * Code generation for function 'indexShapeCheck' 9 | * 10 | */ 11 | 12 | #ifndef INDEXSHAPECHECK_H 13 | #define INDEXSHAPECHECK_H 14 | 15 | /* Include files */ 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include "tmwtypes.h" 21 | #include "mex.h" 22 | #include "emlrt.h" 23 | #include "rtwtypes.h" 24 | #include "ExtractWindowTrajectory_types.h" 25 | 26 | /* Function Declarations */ 27 | extern void indexShapeCheck(const emlrtStack *sp, int32_T matrixSize, const 28 | int32_T indexSize[2]); 29 | 30 | #endif 31 | 32 | /* End of code generation (indexShapeCheck.h) */ 33 | -------------------------------------------------------------------------------- /Controllers/MPC/generated/codegen/mex/ExtractWindowTrajectory/interface/_coder_ExtractWindowTrajectory_api.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Academic License - for use in teaching, academic research, and meeting 3 | * course requirements at degree granting institutions only. Not for 4 | * government, commercial, or other organizational use. 5 | * 6 | * _coder_ExtractWindowTrajectory_api.h 7 | * 8 | * Code generation for function '_coder_ExtractWindowTrajectory_api' 9 | * 10 | */ 11 | 12 | #ifndef _CODER_EXTRACTWINDOWTRAJECTORY_API_H 13 | #define _CODER_EXTRACTWINDOWTRAJECTORY_API_H 14 | 15 | /* Include files */ 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include "tmwtypes.h" 21 | #include "mex.h" 22 | #include "emlrt.h" 23 | #include "rtwtypes.h" 24 | #include "ExtractWindowTrajectory_types.h" 25 | 26 | /* Function Declarations */ 27 | extern void ExtractWindowTrajectory_api(const mxArray * const prhs[9], int32_T 28 | nlhs, const mxArray *plhs[3]); 29 | 30 | #endif 31 | 32 | /* End of code generation (_coder_ExtractWindowTrajectory_api.h) */ 33 | -------------------------------------------------------------------------------- /Controllers/MPC/generated/codegen/mex/ExtractWindowTrajectory/interface/_coder_ExtractWindowTrajectory_info.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Academic License - for use in teaching, academic research, and meeting 3 | * course requirements at degree granting institutions only. Not for 4 | * government, commercial, or other organizational use. 5 | * 6 | * _coder_ExtractWindowTrajectory_info.h 7 | * 8 | * Code generation for function '_coder_ExtractWindowTrajectory_info' 9 | * 10 | */ 11 | 12 | #ifndef _CODER_EXTRACTWINDOWTRAJECTORY_INFO_H 13 | #define _CODER_EXTRACTWINDOWTRAJECTORY_INFO_H 14 | 15 | /* Include files */ 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include "tmwtypes.h" 21 | #include "mex.h" 22 | #include "emlrt.h" 23 | #include "rtwtypes.h" 24 | #include "ExtractWindowTrajectory_types.h" 25 | 26 | /* Function Declarations */ 27 | MEXFUNCTION_LINKAGE mxArray *emlrtMexFcnProperties(void); 28 | extern const mxArray *emlrtMexFcnResolvedFunctionsInfo(void); 29 | 30 | #endif 31 | 32 | /* End of code generation (_coder_ExtractWindowTrajectory_info.h) */ 33 | -------------------------------------------------------------------------------- /Controllers/MPC/generated/codegen/mex/ExtractWindowTrajectory/interface/_coder_ExtractWindowTrajectory_mex.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Academic License - for use in teaching, academic research, and meeting 3 | * course requirements at degree granting institutions only. Not for 4 | * government, commercial, or other organizational use. 5 | * 6 | * _coder_ExtractWindowTrajectory_mex.h 7 | * 8 | * Code generation for function '_coder_ExtractWindowTrajectory_mex' 9 | * 10 | */ 11 | 12 | #ifndef _CODER_EXTRACTWINDOWTRAJECTORY_MEX_H 13 | #define _CODER_EXTRACTWINDOWTRAJECTORY_MEX_H 14 | 15 | /* Include files */ 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include "tmwtypes.h" 21 | #include "mex.h" 22 | #include "emlrt.h" 23 | #include "rtwtypes.h" 24 | #include "ExtractWindowTrajectory_types.h" 25 | 26 | /* Function Declarations */ 27 | extern void mexFunction(int32_T nlhs, mxArray *plhs[], int32_T nrhs, const 28 | mxArray *prhs[]); 29 | extern emlrtCTX mexFunctionCreateRootTLS(void); 30 | 31 | #endif 32 | 33 | /* End of code generation (_coder_ExtractWindowTrajectory_mex.h) */ 34 | -------------------------------------------------------------------------------- /Controllers/MPC/generated/codegen/mex/ExtractWindowTrajectory/power.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Academic License - for use in teaching, academic research, and meeting 3 | * course requirements at degree granting institutions only. Not for 4 | * government, commercial, or other organizational use. 5 | * 6 | * power.h 7 | * 8 | * Code generation for function 'power' 9 | * 10 | */ 11 | 12 | #ifndef POWER_H 13 | #define POWER_H 14 | 15 | /* Include files */ 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include "tmwtypes.h" 21 | #include "mex.h" 22 | #include "emlrt.h" 23 | #include "rtwtypes.h" 24 | #include "ExtractWindowTrajectory_types.h" 25 | 26 | /* Function Declarations */ 27 | extern void power(const emlrtStack *sp, const emxArray_real_T *a, 28 | emxArray_real_T *y); 29 | 30 | #endif 31 | 32 | /* End of code generation (power.h) */ 33 | -------------------------------------------------------------------------------- /Controllers/MPC/generated/codegen/mex/ExtractWindowTrajectory/repmat.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Academic License - for use in teaching, academic research, and meeting 3 | * course requirements at degree granting institutions only. Not for 4 | * government, commercial, or other organizational use. 5 | * 6 | * repmat.h 7 | * 8 | * Code generation for function 'repmat' 9 | * 10 | */ 11 | 12 | #ifndef REPMAT_H 13 | #define REPMAT_H 14 | 15 | /* Include files */ 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include "tmwtypes.h" 21 | #include "mex.h" 22 | #include "emlrt.h" 23 | #include "rtwtypes.h" 24 | #include "ExtractWindowTrajectory_types.h" 25 | 26 | /* Function Declarations */ 27 | extern void b_repmat(const emlrtStack *sp, const emxArray_real_T *a, const 28 | real_T varargin_1[2], emxArray_real_T *b); 29 | extern void repmat(const emlrtStack *sp, const real_T a[2], const real_T 30 | varargin_1[2], emxArray_real_T *b); 31 | 32 | #endif 33 | 34 | /* End of code generation (repmat.h) */ 35 | -------------------------------------------------------------------------------- /Controllers/MPC/generated/codegen/mex/ExtractWindowTrajectory/rt_nonfinite.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Academic License - for use in teaching, academic research, and meeting 3 | * course requirements at degree granting institutions only. Not for 4 | * government, commercial, or other organizational use. 5 | * 6 | * rt_nonfinite.h 7 | * 8 | * Code generation for function 'ExtractWindowTrajectory' 9 | * 10 | */ 11 | 12 | #ifndef RT_NONFINITE_H 13 | #define RT_NONFINITE_H 14 | #define rtInf mxGetInf() 15 | #define rtMinusInf (-mxGetInf()) 16 | #define rtNaN mxGetNaN() 17 | #define rtIsNaN(X) mxIsNaN(X) 18 | #define rtIsInf(X) mxIsInf(X) 19 | #define rtIsNaNF(X) mxIsNaN(X) 20 | #define rtIsInfF(X) mxIsInf(X) 21 | #endif 22 | /* End of code generation (rt_nonfinite.h) */ 23 | -------------------------------------------------------------------------------- /Controllers/MPC/generated/codegen/mex/ExtractWindowTrajectory/rtwtypes.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Academic License - for use in teaching, academic research, and meeting 3 | * course requirements at degree granting institutions only. Not for 4 | * government, commercial, or other organizational use. 5 | * 6 | * rtwtypes.h 7 | * 8 | * Code generation for function 'ExtractWindowTrajectory' 9 | * 10 | */ 11 | 12 | #ifndef RTWTYPES_H 13 | #define RTWTYPES_H 14 | #include "tmwtypes.h" 15 | /* 16 | * TRUE/FALSE definitions 17 | */ 18 | #ifndef TRUE 19 | #define TRUE (1U) 20 | #endif 21 | #ifndef FALSE 22 | #define FALSE (0U) 23 | #endif 24 | #endif 25 | /* End of code generation (rtwtypes.h) */ 26 | -------------------------------------------------------------------------------- /Controllers/MPC/generated/codegen/mex/ExtractWindowTrajectory/scalexpAlloc.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Academic License - for use in teaching, academic research, and meeting 3 | * course requirements at degree granting institutions only. Not for 4 | * government, commercial, or other organizational use. 5 | * 6 | * scalexpAlloc.c 7 | * 8 | * Code generation for function 'scalexpAlloc' 9 | * 10 | */ 11 | 12 | /* Include files */ 13 | #include "rt_nonfinite.h" 14 | #include "ExtractWindowTrajectory.h" 15 | #include "scalexpAlloc.h" 16 | 17 | /* Function Definitions */ 18 | boolean_T dimagree(const emxArray_real_T *z, const emxArray_real_T *varargin_1) 19 | { 20 | boolean_T p; 21 | boolean_T b_p; 22 | int32_T k; 23 | boolean_T exitg1; 24 | int32_T i6; 25 | int32_T i7; 26 | p = true; 27 | b_p = true; 28 | k = 1; 29 | exitg1 = false; 30 | while ((!exitg1) && (k < 3)) { 31 | if (k <= 1) { 32 | i6 = z->size[0]; 33 | i7 = varargin_1->size[0]; 34 | } else { 35 | i6 = 1; 36 | i7 = 1; 37 | } 38 | 39 | if (i6 != i7) { 40 | b_p = false; 41 | exitg1 = true; 42 | } else { 43 | k++; 44 | } 45 | } 46 | 47 | if (!b_p) { 48 | p = false; 49 | } 50 | 51 | return p; 52 | } 53 | 54 | /* End of code generation (scalexpAlloc.c) */ 55 | -------------------------------------------------------------------------------- /Controllers/MPC/generated/codegen/mex/ExtractWindowTrajectory/scalexpAlloc.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Academic License - for use in teaching, academic research, and meeting 3 | * course requirements at degree granting institutions only. Not for 4 | * government, commercial, or other organizational use. 5 | * 6 | * scalexpAlloc.h 7 | * 8 | * Code generation for function 'scalexpAlloc' 9 | * 10 | */ 11 | 12 | #ifndef SCALEXPALLOC_H 13 | #define SCALEXPALLOC_H 14 | 15 | /* Include files */ 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include "tmwtypes.h" 21 | #include "mex.h" 22 | #include "emlrt.h" 23 | #include "rtwtypes.h" 24 | #include "ExtractWindowTrajectory_types.h" 25 | 26 | /* Function Declarations */ 27 | extern boolean_T dimagree(const emxArray_real_T *z, const emxArray_real_T 28 | *varargin_1); 29 | 30 | #endif 31 | 32 | /* End of code generation (scalexpAlloc.h) */ 33 | -------------------------------------------------------------------------------- /Controllers/MPC/generated/codegen/mex/ExtractWindowTrajectory/sort1.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Academic License - for use in teaching, academic research, and meeting 3 | * course requirements at degree granting institutions only. Not for 4 | * government, commercial, or other organizational use. 5 | * 6 | * sort1.h 7 | * 8 | * Code generation for function 'sort1' 9 | * 10 | */ 11 | 12 | #ifndef SORT1_H 13 | #define SORT1_H 14 | 15 | /* Include files */ 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include "tmwtypes.h" 21 | #include "mex.h" 22 | #include "emlrt.h" 23 | #include "rtwtypes.h" 24 | #include "ExtractWindowTrajectory_types.h" 25 | 26 | /* Function Declarations */ 27 | extern void sort(const emlrtStack *sp, emxArray_real_T *x); 28 | 29 | #endif 30 | 31 | /* End of code generation (sort1.h) */ 32 | -------------------------------------------------------------------------------- /Controllers/MPC/generated/codegen/mex/ExtractWindowTrajectory/sortIdx.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Academic License - for use in teaching, academic research, and meeting 3 | * course requirements at degree granting institutions only. Not for 4 | * government, commercial, or other organizational use. 5 | * 6 | * sortIdx.h 7 | * 8 | * Code generation for function 'sortIdx' 9 | * 10 | */ 11 | 12 | #ifndef SORTIDX_H 13 | #define SORTIDX_H 14 | 15 | /* Include files */ 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include "tmwtypes.h" 21 | #include "mex.h" 22 | #include "emlrt.h" 23 | #include "rtwtypes.h" 24 | #include "ExtractWindowTrajectory_types.h" 25 | 26 | /* Function Declarations */ 27 | extern void sortIdx(const emlrtStack *sp, emxArray_real_T *x, emxArray_int32_T 28 | *idx); 29 | 30 | #endif 31 | 32 | /* End of code generation (sortIdx.h) */ 33 | -------------------------------------------------------------------------------- /Controllers/MPC/generated/codegen/mex/FitReferencePathPolynomial/ComputeDerivativePolynomialCoefficients.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Academic License - for use in teaching, academic research, and meeting 3 | * course requirements at degree granting institutions only. Not for 4 | * government, commercial, or other organizational use. 5 | * 6 | * ComputeDerivativePolynomialCoefficients.h 7 | * 8 | * Code generation for function 'ComputeDerivativePolynomialCoefficients' 9 | * 10 | */ 11 | 12 | #ifndef COMPUTEDERIVATIVEPOLYNOMIALCOEFFICIENTS_H 13 | #define COMPUTEDERIVATIVEPOLYNOMIALCOEFFICIENTS_H 14 | 15 | /* Include files */ 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include "tmwtypes.h" 21 | #include "mex.h" 22 | #include "emlrt.h" 23 | #include "rtwtypes.h" 24 | #include "FitReferencePathPolynomial_types.h" 25 | 26 | /* Function Declarations */ 27 | extern void c_ComputeDerivativePolynomialCo(const emlrtStack *sp, const 28 | emxArray_real_T *coeff, emxArray_real_T *dcoeff); 29 | 30 | #endif 31 | 32 | /* End of code generation (ComputeDerivativePolynomialCoefficients.h) */ 33 | -------------------------------------------------------------------------------- /Controllers/MPC/generated/codegen/mex/FitReferencePathPolynomial/ComputeSquaredPolynomialCoefficients.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Academic License - for use in teaching, academic research, and meeting 3 | * course requirements at degree granting institutions only. Not for 4 | * government, commercial, or other organizational use. 5 | * 6 | * ComputeSquaredPolynomialCoefficients.h 7 | * 8 | * Code generation for function 'ComputeSquaredPolynomialCoefficients' 9 | * 10 | */ 11 | 12 | #ifndef COMPUTESQUAREDPOLYNOMIALCOEFFICIENTS_H 13 | #define COMPUTESQUAREDPOLYNOMIALCOEFFICIENTS_H 14 | 15 | /* Include files */ 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include "tmwtypes.h" 21 | #include "mex.h" 22 | #include "emlrt.h" 23 | #include "rtwtypes.h" 24 | #include "FitReferencePathPolynomial_types.h" 25 | 26 | /* Function Declarations */ 27 | extern void c_ComputeSquaredPolynomialCoeff(const emlrtStack *sp, const 28 | emxArray_real_T *coeff, emxArray_real_T *coeff_squared); 29 | 30 | #endif 31 | 32 | /* End of code generation (ComputeSquaredPolynomialCoefficients.h) */ 33 | -------------------------------------------------------------------------------- /Controllers/MPC/generated/codegen/mex/FitReferencePathPolynomial/ConstrainedPolyFit.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Academic License - for use in teaching, academic research, and meeting 3 | * course requirements at degree granting institutions only. Not for 4 | * government, commercial, or other organizational use. 5 | * 6 | * ConstrainedPolyFit.h 7 | * 8 | * Code generation for function 'ConstrainedPolyFit' 9 | * 10 | */ 11 | 12 | #ifndef CONSTRAINEDPOLYFIT_H 13 | #define CONSTRAINEDPOLYFIT_H 14 | 15 | /* Include files */ 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include "tmwtypes.h" 21 | #include "mex.h" 22 | #include "emlrt.h" 23 | #include "rtwtypes.h" 24 | #include "FitReferencePathPolynomial_types.h" 25 | 26 | /* Function Declarations */ 27 | extern void ConstrainedPolyFit(const emlrtStack *sp, const emxArray_real_T *t, 28 | const emxArray_real_T *data, real_T order, emxArray_real_T *polyCoeffs); 29 | extern void b_ConstrainedPolyFit(const emlrtStack *sp, const emxArray_real_T *t, 30 | const emxArray_real_T *data, real_T order, emxArray_real_T *polyCoeffs); 31 | extern void c_ConstrainedPolyFit(const emlrtStack *sp, const real_T t[100], 32 | const real_T data[100], real_T order, emxArray_real_T *polyCoeffs); 33 | 34 | #endif 35 | 36 | /* End of code generation (ConstrainedPolyFit.h) */ 37 | -------------------------------------------------------------------------------- /Controllers/MPC/generated/codegen/mex/FitReferencePathPolynomial/EvaluatePolynomial.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Academic License - for use in teaching, academic research, and meeting 3 | * course requirements at degree granting institutions only. Not for 4 | * government, commercial, or other organizational use. 5 | * 6 | * EvaluatePolynomial.h 7 | * 8 | * Code generation for function 'EvaluatePolynomial' 9 | * 10 | */ 11 | 12 | #ifndef EVALUATEPOLYNOMIAL_H 13 | #define EVALUATEPOLYNOMIAL_H 14 | 15 | /* Include files */ 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include "tmwtypes.h" 21 | #include "mex.h" 22 | #include "emlrt.h" 23 | #include "rtwtypes.h" 24 | #include "FitReferencePathPolynomial_types.h" 25 | 26 | /* Function Declarations */ 27 | extern real_T EvaluatePolynomial(const emlrtStack *sp, const emxArray_real_T 28 | *coeff, real_T x); 29 | extern void b_EvaluatePolynomial(const emlrtStack *sp, const emxArray_real_T 30 | *coeff, const real_T x[100], real_T y[100]); 31 | extern void c_EvaluatePolynomial(const emlrtStack *sp, const emxArray_real_T 32 | *coeff, const emxArray_real_T *x, emxArray_real_T *y); 33 | 34 | #endif 35 | 36 | /* End of code generation (EvaluatePolynomial.h) */ 37 | -------------------------------------------------------------------------------- /Controllers/MPC/generated/codegen/mex/FitReferencePathPolynomial/FitReferencePathPolynomial.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Academic License - for use in teaching, academic research, and meeting 3 | * course requirements at degree granting institutions only. Not for 4 | * government, commercial, or other organizational use. 5 | * 6 | * FitReferencePathPolynomial.h 7 | * 8 | * Code generation for function 'FitReferencePathPolynomial' 9 | * 10 | */ 11 | 12 | #ifndef FITREFERENCEPATHPOLYNOMIAL_H 13 | #define FITREFERENCEPATHPOLYNOMIAL_H 14 | 15 | /* Include files */ 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include "tmwtypes.h" 21 | #include "mex.h" 22 | #include "emlrt.h" 23 | #include "rtwtypes.h" 24 | #include "FitReferencePathPolynomial_types.h" 25 | 26 | /* Function Declarations */ 27 | extern void FitReferencePathPolynomial(const emlrtStack *sp, const 28 | emxArray_real_T *WindowTrajectoryPoints, real_T approximation_order, real_T 29 | velocity, real_T ts, real_T N, emxArray_real_T *TrajectoryPoints, 30 | emxArray_real_T *coeff_xs, emxArray_real_T *coeff_ys, real_T 31 | *windowTrajectoryLength, real_T *minDistancePoint); 32 | 33 | #endif 34 | 35 | /* End of code generation (FitReferencePathPolynomial.h) */ 36 | -------------------------------------------------------------------------------- /Controllers/MPC/generated/codegen/mex/FitReferencePathPolynomial/FitReferencePathPolynomial_initialize.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Academic License - for use in teaching, academic research, and meeting 3 | * course requirements at degree granting institutions only. Not for 4 | * government, commercial, or other organizational use. 5 | * 6 | * FitReferencePathPolynomial_initialize.c 7 | * 8 | * Code generation for function 'FitReferencePathPolynomial_initialize' 9 | * 10 | */ 11 | 12 | /* Include files */ 13 | #include "rt_nonfinite.h" 14 | #include "FitReferencePathPolynomial.h" 15 | #include "FitReferencePathPolynomial_initialize.h" 16 | #include "_coder_FitReferencePathPolynomial_mex.h" 17 | #include "FitReferencePathPolynomial_data.h" 18 | 19 | /* Function Definitions */ 20 | void FitReferencePathPolynomial_initialize(void) 21 | { 22 | emlrtStack st = { NULL, /* site */ 23 | NULL, /* tls */ 24 | NULL /* prev */ 25 | }; 26 | 27 | mexFunctionCreateRootTLS(); 28 | emlrtBreakCheckR2012bFlagVar = emlrtGetBreakCheckFlagAddressR2012b(); 29 | st.tls = emlrtRootTLSGlobal; 30 | emlrtClearAllocCountR2012b(&st, false, 0U, 0); 31 | emlrtEnterRtStackR2012b(&st); 32 | emlrtFirstTimeR2012b(emlrtRootTLSGlobal); 33 | } 34 | 35 | /* End of code generation (FitReferencePathPolynomial_initialize.c) */ 36 | -------------------------------------------------------------------------------- /Controllers/MPC/generated/codegen/mex/FitReferencePathPolynomial/FitReferencePathPolynomial_initialize.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Academic License - for use in teaching, academic research, and meeting 3 | * course requirements at degree granting institutions only. Not for 4 | * government, commercial, or other organizational use. 5 | * 6 | * FitReferencePathPolynomial_initialize.h 7 | * 8 | * Code generation for function 'FitReferencePathPolynomial_initialize' 9 | * 10 | */ 11 | 12 | #ifndef FITREFERENCEPATHPOLYNOMIAL_INITIALIZE_H 13 | #define FITREFERENCEPATHPOLYNOMIAL_INITIALIZE_H 14 | 15 | /* Include files */ 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include "tmwtypes.h" 21 | #include "mex.h" 22 | #include "emlrt.h" 23 | #include "rtwtypes.h" 24 | #include "FitReferencePathPolynomial_types.h" 25 | 26 | /* Function Declarations */ 27 | extern void FitReferencePathPolynomial_initialize(void); 28 | 29 | #endif 30 | 31 | /* End of code generation (FitReferencePathPolynomial_initialize.h) */ 32 | -------------------------------------------------------------------------------- /Controllers/MPC/generated/codegen/mex/FitReferencePathPolynomial/FitReferencePathPolynomial_mex.bat: -------------------------------------------------------------------------------- 1 | @echo off 2 | set MATLAB=C:\PROGRA~1\MATLAB\R2018a 3 | set MATLAB_ARCH=win64 4 | set MATLAB_BIN="C:\Program Files\MATLAB\R2018a\bin" 5 | set ENTRYPOINT=mexFunction 6 | set OUTDIR=.\ 7 | set LIB_NAME=FitReferencePathPolynomial_mex 8 | set MEX_NAME=FitReferencePathPolynomial_mex 9 | set MEX_EXT=.mexw64 10 | call setEnv.bat 11 | echo # Make settings for FitReferencePathPolynomial > FitReferencePathPolynomial_mex.mki 12 | echo COMPILER=%COMPILER%>> FitReferencePathPolynomial_mex.mki 13 | echo COMPFLAGS=%COMPFLAGS%>> FitReferencePathPolynomial_mex.mki 14 | echo OPTIMFLAGS=%OPTIMFLAGS%>> FitReferencePathPolynomial_mex.mki 15 | echo DEBUGFLAGS=%DEBUGFLAGS%>> FitReferencePathPolynomial_mex.mki 16 | echo LINKER=%LINKER%>> FitReferencePathPolynomial_mex.mki 17 | echo LINKFLAGS=%LINKFLAGS%>> FitReferencePathPolynomial_mex.mki 18 | echo LINKOPTIMFLAGS=%LINKOPTIMFLAGS%>> FitReferencePathPolynomial_mex.mki 19 | echo LINKDEBUGFLAGS=%LINKDEBUGFLAGS%>> FitReferencePathPolynomial_mex.mki 20 | echo MATLAB_ARCH=%MATLAB_ARCH%>> FitReferencePathPolynomial_mex.mki 21 | echo OMPFLAGS= >> FitReferencePathPolynomial_mex.mki 22 | echo OMPLINKFLAGS= >> FitReferencePathPolynomial_mex.mki 23 | echo EMC_COMPILER=msvc150>> FitReferencePathPolynomial_mex.mki 24 | echo EMC_CONFIG=optim>> FitReferencePathPolynomial_mex.mki 25 | "C:\Program Files\MATLAB\R2018a\bin\win64\gmake" -j 1 -B -f FitReferencePathPolynomial_mex.mk 26 | -------------------------------------------------------------------------------- /Controllers/MPC/generated/codegen/mex/FitReferencePathPolynomial/FitReferencePathPolynomial_mex.exp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mindThomas/Kugle-MATLAB/b1ee16de78899a131241639d620b5fb7c02a586b/Controllers/MPC/generated/codegen/mex/FitReferencePathPolynomial/FitReferencePathPolynomial_mex.exp -------------------------------------------------------------------------------- /Controllers/MPC/generated/codegen/mex/FitReferencePathPolynomial/FitReferencePathPolynomial_mex.mexw64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mindThomas/Kugle-MATLAB/b1ee16de78899a131241639d620b5fb7c02a586b/Controllers/MPC/generated/codegen/mex/FitReferencePathPolynomial/FitReferencePathPolynomial_mex.mexw64 -------------------------------------------------------------------------------- /Controllers/MPC/generated/codegen/mex/FitReferencePathPolynomial/FitReferencePathPolynomial_mex.mexw64.manifest: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /Controllers/MPC/generated/codegen/mex/FitReferencePathPolynomial/FitReferencePathPolynomial_mex.mki: -------------------------------------------------------------------------------- 1 | # Make settings for FitReferencePathPolynomial 2 | COMPILER=cl 3 | COMPFLAGS=/c /Zp8 /GR /W3 /EHs /nologo /MD /D_CRT_SECURE_NO_DEPRECATE /D_SCL_SECURE_NO_DEPRECATE /D_SECURE_SCL=0 /DMATLAB_MEX_FILE /DMATLAB_MEX_FILE 4 | OPTIMFLAGS=/O2 /Oy- /DNDEBUG 5 | DEBUGFLAGS=/Z7 6 | LINKER=link 7 | LINKFLAGS=/nologo /manifest /export:mexFunction /DLL /LIBPATH:"C:\Program Files\MATLAB\R2018a\extern\lib\win64\microsoft" libmx.lib libmex.lib libmat.lib kernel32.lib user32.lib gdi32.lib winspool.lib comdlg32.lib advapi32.lib shell32.lib ole32.lib oleaut32.lib uuid.lib odbc32.lib odbccp32.lib /EXPORT:mexFunction 8 | LINKOPTIMFLAGS= 9 | LINKDEBUGFLAGS=/debug /PDB:".\FitReferencePathPolynomial_mex.mexw64.pdb" 10 | MATLAB_ARCH=win64 11 | OMPFLAGS= 12 | OMPLINKFLAGS= 13 | EMC_COMPILER=msvc150 14 | EMC_CONFIG=optim 15 | -------------------------------------------------------------------------------- /Controllers/MPC/generated/codegen/mex/FitReferencePathPolynomial/FitReferencePathPolynomial_mex_mex.arf: -------------------------------------------------------------------------------- 1 | libemlrt.lib 2 | libcovrt.lib 3 | libut.lib 4 | libmwmathutil.lib 5 | /export:mexFunction 6 | /export:mexfilerequiredapiversion 7 | /export:emlrtMexFcnProperties 8 | /export:FitReferencePathPolynomial 9 | /export:l_error 10 | /export:emxEnsureCapacity_int32_T 11 | /export:emxEnsureCapacity_real_T 12 | /export:emxEnsureCapacity_real_T1 13 | /export:emxFreeMatrix_cell_wrap_1 14 | /export:emxFree_int32_T 15 | /export:emxFree_real_T 16 | /export:emxInitMatrix_cell_wrap_1 17 | /export:emxInit_int32_T 18 | /export:emxInit_real_T 19 | /export:emxInit_real_T1 20 | /export:FitReferencePathPolynomial_initialize 21 | /export:FitReferencePathPolynomial_terminate 22 | /export:FitReferencePathPolynomial_atexit 23 | -------------------------------------------------------------------------------- /Controllers/MPC/generated/codegen/mex/FitReferencePathPolynomial/FitReferencePathPolynomial_mexutil.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Academic License - for use in teaching, academic research, and meeting 3 | * course requirements at degree granting institutions only. Not for 4 | * government, commercial, or other organizational use. 5 | * 6 | * FitReferencePathPolynomial_mexutil.c 7 | * 8 | * Code generation for function 'FitReferencePathPolynomial_mexutil' 9 | * 10 | */ 11 | 12 | /* Include files */ 13 | #include "rt_nonfinite.h" 14 | #include "FitReferencePathPolynomial.h" 15 | #include "FitReferencePathPolynomial_mexutil.h" 16 | 17 | /* Function Definitions */ 18 | void l_error(const emlrtStack *sp, const mxArray *b, emlrtMCInfo *location) 19 | { 20 | const mxArray *pArray; 21 | pArray = b; 22 | emlrtCallMATLABR2012b(sp, 0, NULL, 1, &pArray, "error", true, location); 23 | } 24 | 25 | /* End of code generation (FitReferencePathPolynomial_mexutil.c) */ 26 | -------------------------------------------------------------------------------- /Controllers/MPC/generated/codegen/mex/FitReferencePathPolynomial/FitReferencePathPolynomial_mexutil.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Academic License - for use in teaching, academic research, and meeting 3 | * course requirements at degree granting institutions only. Not for 4 | * government, commercial, or other organizational use. 5 | * 6 | * FitReferencePathPolynomial_mexutil.h 7 | * 8 | * Code generation for function 'FitReferencePathPolynomial_mexutil' 9 | * 10 | */ 11 | 12 | #ifndef FITREFERENCEPATHPOLYNOMIAL_MEXUTIL_H 13 | #define FITREFERENCEPATHPOLYNOMIAL_MEXUTIL_H 14 | 15 | /* Include files */ 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include "tmwtypes.h" 21 | #include "mex.h" 22 | #include "emlrt.h" 23 | #include "rtwtypes.h" 24 | #include "FitReferencePathPolynomial_types.h" 25 | 26 | /* Function Declarations */ 27 | extern void l_error(const emlrtStack *sp, const mxArray *b, emlrtMCInfo 28 | *location); 29 | 30 | #endif 31 | 32 | /* End of code generation (FitReferencePathPolynomial_mexutil.h) */ 33 | -------------------------------------------------------------------------------- /Controllers/MPC/generated/codegen/mex/FitReferencePathPolynomial/FitReferencePathPolynomial_terminate.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Academic License - for use in teaching, academic research, and meeting 3 | * course requirements at degree granting institutions only. Not for 4 | * government, commercial, or other organizational use. 5 | * 6 | * FitReferencePathPolynomial_terminate.h 7 | * 8 | * Code generation for function 'FitReferencePathPolynomial_terminate' 9 | * 10 | */ 11 | 12 | #ifndef FITREFERENCEPATHPOLYNOMIAL_TERMINATE_H 13 | #define FITREFERENCEPATHPOLYNOMIAL_TERMINATE_H 14 | 15 | /* Include files */ 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include "tmwtypes.h" 21 | #include "mex.h" 22 | #include "emlrt.h" 23 | #include "rtwtypes.h" 24 | #include "FitReferencePathPolynomial_types.h" 25 | 26 | /* Function Declarations */ 27 | extern void FitReferencePathPolynomial_atexit(void); 28 | extern void FitReferencePathPolynomial_terminate(void); 29 | 30 | #endif 31 | 32 | /* End of code generation (FitReferencePathPolynomial_terminate.h) */ 33 | -------------------------------------------------------------------------------- /Controllers/MPC/generated/codegen/mex/FitReferencePathPolynomial/buildInfo.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mindThomas/Kugle-MATLAB/b1ee16de78899a131241639d620b5fb7c02a586b/Controllers/MPC/generated/codegen/mex/FitReferencePathPolynomial/buildInfo.mat -------------------------------------------------------------------------------- /Controllers/MPC/generated/codegen/mex/FitReferencePathPolynomial/c_mexapi_version.c: -------------------------------------------------------------------------------- 1 | #include 2 | #if !defined(USE_MEX_CMD) || defined(MATLAB_MEXCMD_RELEASE) || defined(MX_COMPAT_32) || defined(MX_COMPAT_64) || TARGET_API_VERSION >= 800 3 | #define MX_TMP_TARGET_API_VER MX_TARGET_API_VER 4 | #else 5 | #define MX_TMP_TARGET_API_VER 0 6 | #endif 7 | MEXFUNCTION_LINKAGE 8 | void mexfilerequiredapiversion(unsigned int* built_by_rel, unsigned int* target_api_ver) 9 | { 10 | *built_by_rel = 0x2018a; 11 | *target_api_ver = MX_TMP_TARGET_API_VER; 12 | } 13 | 14 | -------------------------------------------------------------------------------- /Controllers/MPC/generated/codegen/mex/FitReferencePathPolynomial/diag.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Academic License - for use in teaching, academic research, and meeting 3 | * course requirements at degree granting institutions only. Not for 4 | * government, commercial, or other organizational use. 5 | * 6 | * diag.h 7 | * 8 | * Code generation for function 'diag' 9 | * 10 | */ 11 | 12 | #ifndef DIAG_H 13 | #define DIAG_H 14 | 15 | /* Include files */ 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include "tmwtypes.h" 21 | #include "mex.h" 22 | #include "emlrt.h" 23 | #include "rtwtypes.h" 24 | #include "FitReferencePathPolynomial_types.h" 25 | 26 | /* Function Declarations */ 27 | extern void b_diag(const emlrtStack *sp, const emxArray_real_T *v, 28 | emxArray_real_T *d); 29 | extern void diag(const emlrtStack *sp, const emxArray_real_T *v, emxArray_real_T 30 | *d); 31 | 32 | #endif 33 | 34 | /* End of code generation (diag.h) */ 35 | -------------------------------------------------------------------------------- /Controllers/MPC/generated/codegen/mex/FitReferencePathPolynomial/diff.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Academic License - for use in teaching, academic research, and meeting 3 | * course requirements at degree granting institutions only. Not for 4 | * government, commercial, or other organizational use. 5 | * 6 | * diff.h 7 | * 8 | * Code generation for function 'diff' 9 | * 10 | */ 11 | 12 | #ifndef DIFF_H 13 | #define DIFF_H 14 | 15 | /* Include files */ 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include "tmwtypes.h" 21 | #include "mex.h" 22 | #include "emlrt.h" 23 | #include "rtwtypes.h" 24 | #include "FitReferencePathPolynomial_types.h" 25 | 26 | /* Function Declarations */ 27 | extern void diff(const emlrtStack *sp, const emxArray_real_T *x, emxArray_real_T 28 | *y); 29 | 30 | #endif 31 | 32 | /* End of code generation (diff.h) */ 33 | -------------------------------------------------------------------------------- /Controllers/MPC/generated/codegen/mex/FitReferencePathPolynomial/eml_int_forloop_overflow_check.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Academic License - for use in teaching, academic research, and meeting 3 | * course requirements at degree granting institutions only. Not for 4 | * government, commercial, or other organizational use. 5 | * 6 | * eml_int_forloop_overflow_check.c 7 | * 8 | * Code generation for function 'eml_int_forloop_overflow_check' 9 | * 10 | */ 11 | 12 | /* Include files */ 13 | #include "rt_nonfinite.h" 14 | #include "FitReferencePathPolynomial.h" 15 | #include "eml_int_forloop_overflow_check.h" 16 | 17 | /* Variable Definitions */ 18 | static emlrtRTEInfo ob_emlrtRTEI = { 88,/* lineNo */ 19 | 9, /* colNo */ 20 | "eml_int_forloop_overflow_check", /* fName */ 21 | "C:\\Program Files\\MATLAB\\R2018a\\toolbox\\eml\\lib\\matlab\\eml\\eml_int_forloop_overflow_check.m"/* pName */ 22 | }; 23 | 24 | /* Function Definitions */ 25 | void check_forloop_overflow_error(const emlrtStack *sp) 26 | { 27 | emlrtErrorWithMessageIdR2018a(sp, &ob_emlrtRTEI, 28 | "Coder:toolbox:int_forloop_overflow", "Coder:toolbox:int_forloop_overflow", 29 | 3, 4, 5, "int32"); 30 | } 31 | 32 | /* End of code generation (eml_int_forloop_overflow_check.c) */ 33 | -------------------------------------------------------------------------------- /Controllers/MPC/generated/codegen/mex/FitReferencePathPolynomial/eml_int_forloop_overflow_check.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Academic License - for use in teaching, academic research, and meeting 3 | * course requirements at degree granting institutions only. Not for 4 | * government, commercial, or other organizational use. 5 | * 6 | * eml_int_forloop_overflow_check.h 7 | * 8 | * Code generation for function 'eml_int_forloop_overflow_check' 9 | * 10 | */ 11 | 12 | #ifndef EML_INT_FORLOOP_OVERFLOW_CHECK_H 13 | #define EML_INT_FORLOOP_OVERFLOW_CHECK_H 14 | 15 | /* Include files */ 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include "tmwtypes.h" 21 | #include "mex.h" 22 | #include "emlrt.h" 23 | #include "rtwtypes.h" 24 | #include "FitReferencePathPolynomial_types.h" 25 | 26 | /* Function Declarations */ 27 | extern void check_forloop_overflow_error(const emlrtStack *sp); 28 | 29 | #endif 30 | 31 | /* End of code generation (eml_int_forloop_overflow_check.h) */ 32 | -------------------------------------------------------------------------------- /Controllers/MPC/generated/codegen/mex/FitReferencePathPolynomial/error.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Academic License - for use in teaching, academic research, and meeting 3 | * course requirements at degree granting institutions only. Not for 4 | * government, commercial, or other organizational use. 5 | * 6 | * error.h 7 | * 8 | * Code generation for function 'error' 9 | * 10 | */ 11 | 12 | #ifndef ERROR_H 13 | #define ERROR_H 14 | 15 | /* Include files */ 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include "tmwtypes.h" 21 | #include "mex.h" 22 | #include "emlrt.h" 23 | #include "rtwtypes.h" 24 | #include "FitReferencePathPolynomial_types.h" 25 | 26 | /* Function Declarations */ 27 | extern void b_error(const emlrtStack *sp); 28 | extern void error(const emlrtStack *sp); 29 | extern void h_error(const emlrtStack *sp); 30 | extern void i_error(const emlrtStack *sp, int32_T varargin_2); 31 | extern void j_error(const emlrtStack *sp, int32_T varargin_2); 32 | extern void k_error(const emlrtStack *sp); 33 | 34 | #endif 35 | 36 | /* End of code generation (error.h) */ 37 | -------------------------------------------------------------------------------- /Controllers/MPC/generated/codegen/mex/FitReferencePathPolynomial/error1.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Academic License - for use in teaching, academic research, and meeting 3 | * course requirements at degree granting institutions only. Not for 4 | * government, commercial, or other organizational use. 5 | * 6 | * error1.h 7 | * 8 | * Code generation for function 'error1' 9 | * 10 | */ 11 | 12 | #ifndef ERROR1_H 13 | #define ERROR1_H 14 | 15 | /* Include files */ 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include "tmwtypes.h" 21 | #include "mex.h" 22 | #include "emlrt.h" 23 | #include "rtwtypes.h" 24 | #include "FitReferencePathPolynomial_types.h" 25 | 26 | /* Function Declarations */ 27 | extern void c_error(const emlrtStack *sp); 28 | extern void d_error(const emlrtStack *sp); 29 | extern void e_error(const emlrtStack *sp); 30 | extern void f_error(const emlrtStack *sp); 31 | extern void g_error(const emlrtStack *sp); 32 | 33 | #endif 34 | 35 | /* End of code generation (error1.h) */ 36 | -------------------------------------------------------------------------------- /Controllers/MPC/generated/codegen/mex/FitReferencePathPolynomial/interface/_coder_FitReferencePathPolynomial_api.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Academic License - for use in teaching, academic research, and meeting 3 | * course requirements at degree granting institutions only. Not for 4 | * government, commercial, or other organizational use. 5 | * 6 | * _coder_FitReferencePathPolynomial_api.h 7 | * 8 | * Code generation for function '_coder_FitReferencePathPolynomial_api' 9 | * 10 | */ 11 | 12 | #ifndef _CODER_FITREFERENCEPATHPOLYNOMIAL_API_H 13 | #define _CODER_FITREFERENCEPATHPOLYNOMIAL_API_H 14 | 15 | /* Include files */ 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include "tmwtypes.h" 21 | #include "mex.h" 22 | #include "emlrt.h" 23 | #include "rtwtypes.h" 24 | #include "FitReferencePathPolynomial_types.h" 25 | 26 | /* Function Declarations */ 27 | extern void FitReferencePathPolynomial_api(const mxArray * const prhs[5], 28 | int32_T nlhs, const mxArray *plhs[5]); 29 | 30 | #endif 31 | 32 | /* End of code generation (_coder_FitReferencePathPolynomial_api.h) */ 33 | -------------------------------------------------------------------------------- /Controllers/MPC/generated/codegen/mex/FitReferencePathPolynomial/interface/_coder_FitReferencePathPolynomial_info.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Academic License - for use in teaching, academic research, and meeting 3 | * course requirements at degree granting institutions only. Not for 4 | * government, commercial, or other organizational use. 5 | * 6 | * _coder_FitReferencePathPolynomial_info.h 7 | * 8 | * Code generation for function '_coder_FitReferencePathPolynomial_info' 9 | * 10 | */ 11 | 12 | #ifndef _CODER_FITREFERENCEPATHPOLYNOMIAL_INFO_H 13 | #define _CODER_FITREFERENCEPATHPOLYNOMIAL_INFO_H 14 | 15 | /* Include files */ 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include "tmwtypes.h" 21 | #include "mex.h" 22 | #include "emlrt.h" 23 | #include "rtwtypes.h" 24 | #include "FitReferencePathPolynomial_types.h" 25 | 26 | /* Function Declarations */ 27 | MEXFUNCTION_LINKAGE mxArray *emlrtMexFcnProperties(void); 28 | extern const mxArray *emlrtMexFcnResolvedFunctionsInfo(void); 29 | 30 | #endif 31 | 32 | /* End of code generation (_coder_FitReferencePathPolynomial_info.h) */ 33 | -------------------------------------------------------------------------------- /Controllers/MPC/generated/codegen/mex/FitReferencePathPolynomial/interface/_coder_FitReferencePathPolynomial_mex.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Academic License - for use in teaching, academic research, and meeting 3 | * course requirements at degree granting institutions only. Not for 4 | * government, commercial, or other organizational use. 5 | * 6 | * _coder_FitReferencePathPolynomial_mex.h 7 | * 8 | * Code generation for function '_coder_FitReferencePathPolynomial_mex' 9 | * 10 | */ 11 | 12 | #ifndef _CODER_FITREFERENCEPATHPOLYNOMIAL_MEX_H 13 | #define _CODER_FITREFERENCEPATHPOLYNOMIAL_MEX_H 14 | 15 | /* Include files */ 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include "tmwtypes.h" 21 | #include "mex.h" 22 | #include "emlrt.h" 23 | #include "rtwtypes.h" 24 | #include "FitReferencePathPolynomial_types.h" 25 | 26 | /* Function Declarations */ 27 | extern void mexFunction(int32_T nlhs, mxArray *plhs[], int32_T nrhs, const 28 | mxArray *prhs[]); 29 | extern emlrtCTX mexFunctionCreateRootTLS(void); 30 | 31 | #endif 32 | 33 | /* End of code generation (_coder_FitReferencePathPolynomial_mex.h) */ 34 | -------------------------------------------------------------------------------- /Controllers/MPC/generated/codegen/mex/FitReferencePathPolynomial/linspace.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Academic License - for use in teaching, academic research, and meeting 3 | * course requirements at degree granting institutions only. Not for 4 | * government, commercial, or other organizational use. 5 | * 6 | * linspace.c 7 | * 8 | * Code generation for function 'linspace' 9 | * 10 | */ 11 | 12 | /* Include files */ 13 | #include "mwmathutil.h" 14 | #include "rt_nonfinite.h" 15 | #include "FitReferencePathPolynomial.h" 16 | #include "linspace.h" 17 | 18 | /* Function Definitions */ 19 | void linspace(real_T d2, real_T y[100]) 20 | { 21 | real_T delta1; 22 | int32_T k; 23 | y[99] = d2; 24 | y[0] = 0.0; 25 | if ((d2 < 0.0) && (muDoubleScalarAbs(d2) > 8.9884656743115785E+307)) { 26 | delta1 = d2 / 99.0; 27 | for (k = 0; k < 98; k++) { 28 | y[k + 1] = delta1 * (1.0 + (real_T)k); 29 | } 30 | } else { 31 | delta1 = d2 / 99.0; 32 | for (k = 0; k < 98; k++) { 33 | y[k + 1] = (1.0 + (real_T)k) * delta1; 34 | } 35 | } 36 | } 37 | 38 | /* End of code generation (linspace.c) */ 39 | -------------------------------------------------------------------------------- /Controllers/MPC/generated/codegen/mex/FitReferencePathPolynomial/linspace.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Academic License - for use in teaching, academic research, and meeting 3 | * course requirements at degree granting institutions only. Not for 4 | * government, commercial, or other organizational use. 5 | * 6 | * linspace.h 7 | * 8 | * Code generation for function 'linspace' 9 | * 10 | */ 11 | 12 | #ifndef LINSPACE_H 13 | #define LINSPACE_H 14 | 15 | /* Include files */ 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include "tmwtypes.h" 21 | #include "mex.h" 22 | #include "emlrt.h" 23 | #include "rtwtypes.h" 24 | #include "FitReferencePathPolynomial_types.h" 25 | 26 | /* Function Declarations */ 27 | extern void linspace(real_T d2, real_T y[100]); 28 | 29 | #endif 30 | 31 | /* End of code generation (linspace.h) */ 32 | -------------------------------------------------------------------------------- /Controllers/MPC/generated/codegen/mex/FitReferencePathPolynomial/power.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Academic License - for use in teaching, academic research, and meeting 3 | * course requirements at degree granting institutions only. Not for 4 | * government, commercial, or other organizational use. 5 | * 6 | * power.h 7 | * 8 | * Code generation for function 'power' 9 | * 10 | */ 11 | 12 | #ifndef POWER_H 13 | #define POWER_H 14 | 15 | /* Include files */ 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include "tmwtypes.h" 21 | #include "mex.h" 22 | #include "emlrt.h" 23 | #include "rtwtypes.h" 24 | #include "FitReferencePathPolynomial_types.h" 25 | 26 | /* Function Declarations */ 27 | extern void b_power(const emlrtStack *sp, const emxArray_real_T *a, real_T b, 28 | emxArray_real_T *y); 29 | extern void c_power(const emlrtStack *sp, real_T a, const emxArray_real_T *b, 30 | emxArray_real_T *y); 31 | extern void d_power(const emlrtStack *sp, const real_T a[100], real_T b, real_T 32 | y[100]); 33 | extern void power(const emlrtStack *sp, const emxArray_real_T *a, 34 | emxArray_real_T *y); 35 | 36 | #endif 37 | 38 | /* End of code generation (power.h) */ 39 | -------------------------------------------------------------------------------- /Controllers/MPC/generated/codegen/mex/FitReferencePathPolynomial/rdivide.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Academic License - for use in teaching, academic research, and meeting 3 | * course requirements at degree granting institutions only. Not for 4 | * government, commercial, or other organizational use. 5 | * 6 | * rdivide.c 7 | * 8 | * Code generation for function 'rdivide' 9 | * 10 | */ 11 | 12 | /* Include files */ 13 | #include "rt_nonfinite.h" 14 | #include "FitReferencePathPolynomial.h" 15 | #include "rdivide.h" 16 | #include "FitReferencePathPolynomial_emxutil.h" 17 | 18 | /* Variable Definitions */ 19 | static emlrtRTEInfo gb_emlrtRTEI = { 1,/* lineNo */ 20 | 14, /* colNo */ 21 | "rdivide", /* fName */ 22 | "C:\\Program Files\\MATLAB\\R2018a\\toolbox\\eml\\lib\\matlab\\ops\\rdivide.m"/* pName */ 23 | }; 24 | 25 | /* Function Definitions */ 26 | void rdivide(const emlrtStack *sp, const emxArray_real_T *y, emxArray_real_T *z) 27 | { 28 | int32_T i9; 29 | int32_T loop_ub; 30 | i9 = z->size[0]; 31 | z->size[0] = y->size[0]; 32 | emxEnsureCapacity_real_T(sp, z, i9, &gb_emlrtRTEI); 33 | loop_ub = y->size[0]; 34 | for (i9 = 0; i9 < loop_ub; i9++) { 35 | z->data[i9] = 1.0 / y->data[i9]; 36 | } 37 | } 38 | 39 | /* End of code generation (rdivide.c) */ 40 | -------------------------------------------------------------------------------- /Controllers/MPC/generated/codegen/mex/FitReferencePathPolynomial/rdivide.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Academic License - for use in teaching, academic research, and meeting 3 | * course requirements at degree granting institutions only. Not for 4 | * government, commercial, or other organizational use. 5 | * 6 | * rdivide.h 7 | * 8 | * Code generation for function 'rdivide' 9 | * 10 | */ 11 | 12 | #ifndef RDIVIDE_H 13 | #define RDIVIDE_H 14 | 15 | /* Include files */ 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include "tmwtypes.h" 21 | #include "mex.h" 22 | #include "emlrt.h" 23 | #include "rtwtypes.h" 24 | #include "FitReferencePathPolynomial_types.h" 25 | 26 | /* Function Declarations */ 27 | extern void rdivide(const emlrtStack *sp, const emxArray_real_T *y, 28 | emxArray_real_T *z); 29 | 30 | #endif 31 | 32 | /* End of code generation (rdivide.h) */ 33 | -------------------------------------------------------------------------------- /Controllers/MPC/generated/codegen/mex/FitReferencePathPolynomial/rt_nonfinite.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Academic License - for use in teaching, academic research, and meeting 3 | * course requirements at degree granting institutions only. Not for 4 | * government, commercial, or other organizational use. 5 | * 6 | * rt_nonfinite.h 7 | * 8 | * Code generation for function 'FitReferencePathPolynomial' 9 | * 10 | */ 11 | 12 | #ifndef RT_NONFINITE_H 13 | #define RT_NONFINITE_H 14 | #define rtInf mxGetInf() 15 | #define rtMinusInf (-mxGetInf()) 16 | #define rtNaN mxGetNaN() 17 | #define rtIsNaN(X) mxIsNaN(X) 18 | #define rtIsInf(X) mxIsInf(X) 19 | #define rtIsNaNF(X) mxIsNaN(X) 20 | #define rtIsInfF(X) mxIsInf(X) 21 | #endif 22 | /* End of code generation (rt_nonfinite.h) */ 23 | -------------------------------------------------------------------------------- /Controllers/MPC/generated/codegen/mex/FitReferencePathPolynomial/rtwtypes.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Academic License - for use in teaching, academic research, and meeting 3 | * course requirements at degree granting institutions only. Not for 4 | * government, commercial, or other organizational use. 5 | * 6 | * rtwtypes.h 7 | * 8 | * Code generation for function 'FitReferencePathPolynomial' 9 | * 10 | */ 11 | 12 | #ifndef RTWTYPES_H 13 | #define RTWTYPES_H 14 | #include "tmwtypes.h" 15 | /* 16 | * TRUE/FALSE definitions 17 | */ 18 | #ifndef TRUE 19 | #define TRUE (1U) 20 | #endif 21 | #ifndef FALSE 22 | #define FALSE (0U) 23 | #endif 24 | #endif 25 | /* End of code generation (rtwtypes.h) */ 26 | -------------------------------------------------------------------------------- /Controllers/MPC/generated/codegen/mex/FitReferencePathPolynomial/scalexpAlloc.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Academic License - for use in teaching, academic research, and meeting 3 | * course requirements at degree granting institutions only. Not for 4 | * government, commercial, or other organizational use. 5 | * 6 | * scalexpAlloc.h 7 | * 8 | * Code generation for function 'scalexpAlloc' 9 | * 10 | */ 11 | 12 | #ifndef SCALEXPALLOC_H 13 | #define SCALEXPALLOC_H 14 | 15 | /* Include files */ 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include "tmwtypes.h" 21 | #include "mex.h" 22 | #include "emlrt.h" 23 | #include "rtwtypes.h" 24 | #include "FitReferencePathPolynomial_types.h" 25 | 26 | /* Function Declarations */ 27 | extern boolean_T b_dimagree(const emxArray_real_T *z, const emxArray_real_T 28 | *varargin_2); 29 | extern boolean_T dimagree(const emxArray_real_T *z, const emxArray_real_T 30 | *varargin_1); 31 | 32 | #endif 33 | 34 | /* End of code generation (scalexpAlloc.h) */ 35 | -------------------------------------------------------------------------------- /Controllers/MPC/generated/codegen/mex/FitReferencePathPolynomial/svd.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Academic License - for use in teaching, academic research, and meeting 3 | * course requirements at degree granting institutions only. Not for 4 | * government, commercial, or other organizational use. 5 | * 6 | * svd.h 7 | * 8 | * Code generation for function 'svd' 9 | * 10 | */ 11 | 12 | #ifndef SVD_H 13 | #define SVD_H 14 | 15 | /* Include files */ 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include "tmwtypes.h" 21 | #include "mex.h" 22 | #include "emlrt.h" 23 | #include "rtwtypes.h" 24 | #include "FitReferencePathPolynomial_types.h" 25 | 26 | /* Function Declarations */ 27 | extern void svd(const emlrtStack *sp, const emxArray_real_T *A, emxArray_real_T * 28 | U, emxArray_real_T *S, emxArray_real_T *V); 29 | 30 | #endif 31 | 32 | /* End of code generation (svd.h) */ 33 | -------------------------------------------------------------------------------- /Controllers/MPC/generated/codegen/mex/FitReferencePathPolynomial/svd1.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Academic License - for use in teaching, academic research, and meeting 3 | * course requirements at degree granting institutions only. Not for 4 | * government, commercial, or other organizational use. 5 | * 6 | * svd1.h 7 | * 8 | * Code generation for function 'svd1' 9 | * 10 | */ 11 | 12 | #ifndef SVD1_H 13 | #define SVD1_H 14 | 15 | /* Include files */ 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include "tmwtypes.h" 21 | #include "mex.h" 22 | #include "emlrt.h" 23 | #include "rtwtypes.h" 24 | #include "FitReferencePathPolynomial_types.h" 25 | 26 | /* Type Definitions */ 27 | 28 | /* Function Declarations */ 29 | extern void b_svd(const emlrtStack *sp, const emxArray_real_T *A, 30 | emxArray_real_T *U, emxArray_real_T *s, emxArray_real_T *V); 31 | 32 | #endif 33 | 34 | /* End of code generation (svd1.h) */ 35 | -------------------------------------------------------------------------------- /Controllers/MPC/generated/kugle_mpc_obstacles.mexw64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mindThomas/Kugle-MATLAB/b1ee16de78899a131241639d620b5fb7c02a586b/Controllers/MPC/generated/kugle_mpc_obstacles.mexw64 -------------------------------------------------------------------------------- /Controllers/MPC/generated/kugle_mpc_obstacles_codegen.mexw64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mindThomas/Kugle-MATLAB/b1ee16de78899a131241639d620b5fb7c02a586b/Controllers/MPC/generated/kugle_mpc_obstacles_codegen.mexw64 -------------------------------------------------------------------------------- /Controllers/MPC/generated/kugle_mpc_obstacles_codegen/acado_solver.mexw64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mindThomas/Kugle-MATLAB/b1ee16de78899a131241639d620b5fb7c02a586b/Controllers/MPC/generated/kugle_mpc_obstacles_codegen/acado_solver.mexw64 -------------------------------------------------------------------------------- /Controllers/MPC/generated/kugle_mpc_obstacles_codegen/qpoases3/bin/empty.txt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mindThomas/Kugle-MATLAB/b1ee16de78899a131241639d620b5fb7c02a586b/Controllers/MPC/generated/kugle_mpc_obstacles_codegen/qpoases3/bin/empty.txt -------------------------------------------------------------------------------- /Controllers/MPC/generated/kugle_mpc_obstacles_codegen/qpoases3/interfaces/matlab/example1.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mindThomas/Kugle-MATLAB/b1ee16de78899a131241639d620b5fb7c02a586b/Controllers/MPC/generated/kugle_mpc_obstacles_codegen/qpoases3/interfaces/matlab/example1.mat -------------------------------------------------------------------------------- /Controllers/MPC/generated/kugle_mpc_obstacles_codegen/qpoases3/interfaces/matlab/example1a.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mindThomas/Kugle-MATLAB/b1ee16de78899a131241639d620b5fb7c02a586b/Controllers/MPC/generated/kugle_mpc_obstacles_codegen/qpoases3/interfaces/matlab/example1a.mat -------------------------------------------------------------------------------- /Controllers/MPC/generated/kugle_mpc_obstacles_codegen/qpoases3/interfaces/matlab/example1b.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mindThomas/Kugle-MATLAB/b1ee16de78899a131241639d620b5fb7c02a586b/Controllers/MPC/generated/kugle_mpc_obstacles_codegen/qpoases3/interfaces/matlab/example1b.mat -------------------------------------------------------------------------------- /Controllers/MPC/generated/kugle_mpc_obstacles_codegen/qpoases3/make.mk: -------------------------------------------------------------------------------- 1 | ## 2 | ## This file is part of qpOASES. 3 | ## 4 | ## qpOASES -- An Implementation of the Online Active Set Strategy. 5 | ## Copyright (C) 2007-2015 by Hans Joachim Ferreau, Andreas Potschka, 6 | ## Christian Kirches et al. All rights reserved. 7 | ## 8 | ## qpOASES is free software; you can redistribute it and/or 9 | ## modify it under the terms of the GNU Lesser General Public 10 | ## License as published by the Free Software Foundation; either 11 | ## version 2.1 of the License, or (at your option) any later version. 12 | ## 13 | ## qpOASES is distributed in the hope that it will be useful, 14 | ## but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | ## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 16 | ## See the GNU Lesser General Public License for more details. 17 | ## 18 | ## You should have received a copy of the GNU Lesser General Public 19 | ## License along with qpOASES; if not, write to the Free Software 20 | ## Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 21 | ## 22 | 23 | 24 | 25 | ## 26 | ## Filename: make.mk 27 | ## Author: Hans Joachim Ferreau, Andreas Potschka, Christian Kirches 28 | ## Version: 3.1embedded 29 | ## Date: 2007-2015 30 | ## 31 | 32 | 33 | TOP = $(realpath $(dir $(lastword $(MAKEFILE_LIST)))) 34 | 35 | include ${TOP}/make_linux.mk 36 | #include ${TOP}/make_cygwin.mk 37 | #include ${TOP}/make_windows.mk 38 | #include ${TOP}/make_osx.mk 39 | -------------------------------------------------------------------------------- /Controllers/MPC/generated/kugle_mpc_obstacles_codegen/qpoases3/testing/c/data/fetch_cpp_data: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | 3 | ## 4 | ## This file is part of qpOASES. 5 | ## 6 | ## qpOASES -- An Implementation of the Online Active Set Strategy. 7 | ## Copyright (C) 2007-2015 by Hans Joachim Ferreau, Andreas Potschka, 8 | ## Christian Kirches et al. All rights reserved. 9 | ## 10 | ## qpOASES is free software; you can redistribute it and/or 11 | ## modify it under the terms of the GNU Lesser General Public 12 | ## License as published by the Free Software Foundation; either 13 | ## version 2.1 of the License, or (at your option) any later version. 14 | ## 15 | ## qpOASES is distributed in the hope that it will be useful, 16 | ## but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | ## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 18 | ## See the GNU Lesser General Public License for more details. 19 | ## 20 | ## You should have received a copy of the GNU Lesser General Public 21 | ## License along with qpOASES; if not, write to the Free Software 22 | ## Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 23 | ## 24 | 25 | 26 | 27 | ## 28 | ## Filename: testing/c/data/fetch_cpp_data 29 | ## Author: Hans Joachim Ferreau 30 | ## Version: 3.1embedded 31 | ## Date: 2014-2015 32 | 33 | 34 | svn export https://projects.coin-or.org/svn/qpOASES/misc/testingdata/cpp . --force 35 | -------------------------------------------------------------------------------- /Controllers/MPC/generated/kugle_mpc_obstacles_codegen_RUN.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mindThomas/Kugle-MATLAB/b1ee16de78899a131241639d620b5fb7c02a586b/Controllers/MPC/generated/kugle_mpc_obstacles_codegen_RUN.m -------------------------------------------------------------------------------- /Controllers/MPC/generated/kugle_mpc_obstacles_codegen_RUN.mexw64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mindThomas/Kugle-MATLAB/b1ee16de78899a131241639d620b5fb7c02a586b/Controllers/MPC/generated/kugle_mpc_obstacles_codegen_RUN.mexw64 -------------------------------------------------------------------------------- /Controllers/MPC/generated/kugle_mpc_obstacles_codegen_data_acadodata_M1.txt: -------------------------------------------------------------------------------- 1 | 1 0 0 0 0 0 0 0 0 0 0 0 0 0 2 | 0 1 0 0 0 0 0 0 0 0 0 0 0 0 3 | 0 0 1 0 0 0 0 0 0 0 0 0 0 0 4 | 0 0 0 1 0 0 0 0 0 0 0 0 0 0 5 | 0 0 0 0 1 0 0 0 0 0 0 0 0 0 6 | 0 0 0 0 0 1 0 0 0 0 0 0 0 0 7 | 0 0 0 0 0 0 1 0 0 0 0 0 0 0 8 | 0 0 0 0 0 0 0 1 0 0 0 0 0 0 9 | 0 0 0 0 0 0 0 0 1 0 0 0 0 0 10 | 0 0 0 0 0 0 0 0 0 1 0 0 0 0 11 | 0 0 0 0 0 0 0 0 0 0 1 0 0 0 12 | 0 0 0 0 0 0 0 0 0 0 0 1 0 0 13 | 0 0 0 0 0 0 0 0 0 0 0 0 1 0 14 | 0 0 0 0 0 0 0 0 0 0 0 0 0 1 15 | -------------------------------------------------------------------------------- /Controllers/MPC/generated/kugle_mpc_obstacles_codegen_data_acadodata_M2.txt: -------------------------------------------------------------------------------- 1 | 1 0 0 0 0 0 0 0 0 2 | 0 1 0 0 0 0 0 0 0 3 | 0 0 1 0 0 0 0 0 0 4 | 0 0 0 1 0 0 0 0 0 5 | 0 0 0 0 1 0 0 0 0 6 | 0 0 0 0 0 1 0 0 0 7 | 0 0 0 0 0 0 1 0 0 8 | 0 0 0 0 0 0 0 1 0 9 | 0 0 0 0 0 0 0 0 1 10 | -------------------------------------------------------------------------------- /Controllers/MPC/generated/kugle_mpc_obstacles_codegen_data_acadodata_V1.txt: -------------------------------------------------------------------------------- 1 | 1 2 | 0 3 | 0 4 | 0 5 | 0 6 | 0 7 | 0 8 | 0 9 | 0 10 | 0 11 | -------------------------------------------------------------------------------- /Controllers/MPC/generated/kugle_mpc_obstacles_codegen_data_acadodata_V2.txt: -------------------------------------------------------------------------------- 1 | 1 2 | 0 3 | 0 4 | 0 5 | 0 6 | 0 7 | -------------------------------------------------------------------------------- /Controllers/VelocityLQR/generated/VelocityLQRgain.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mindThomas/Kugle-MATLAB/b1ee16de78899a131241639d620b5fb7c02a586b/Controllers/VelocityLQR/generated/VelocityLQRgain.mat -------------------------------------------------------------------------------- /DataProcessing/README.md: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mindThomas/Kugle-MATLAB/b1ee16de78899a131241639d620b5fb7c02a586b/DataProcessing/README.md -------------------------------------------------------------------------------- /DataProcessing/functions/Downsample.m: -------------------------------------------------------------------------------- 1 | function out = Downsample(in, nthSampleToKeep) 2 | 3 | if (isstruct(in)) 4 | out = []; 5 | f = fieldnames(in); 6 | for i = 1:length(f) 7 | out.(f{i}) = in.(f{i})(1:nthSampleToKeep:end,:); 8 | end 9 | else 10 | out = in(1:nthSampleToKeep:end,:); 11 | end 12 | end -------------------------------------------------------------------------------- /DataProcessing/functions/ParseCovarianceDumpArray.m: -------------------------------------------------------------------------------- 1 | function out = ParseCovarianceDumpArray(array) 2 | if (size(array,2) ~= 50) 3 | error('Incorrect dump array size when parsing'); 4 | end 5 | out.time = array(:,1); 6 | out.Cov_q = array(:,2:2+16-1); 7 | out.Cov_dq = array(:,18:18+16-1); 8 | out.Cov_bias = array(:,34:34+9-1); 9 | out.Cov_velocity = array(:,43:43+4-1); 10 | out.Cov_COM = array(:,47:47+4-1); 11 | end 12 | -------------------------------------------------------------------------------- /DataProcessing/functions/RemoveTime.m: -------------------------------------------------------------------------------- 1 | function out = RemoveTime(in, tRemoveStart, tRemoveEnd) 2 | 3 | % trim output data 4 | idxStart = find(in.time >= tRemoveStart); idxStart = idxStart(1); 5 | idxEnd = find(in.time <= tRemoveEnd); idxEnd = idxEnd(end); 6 | 7 | if ((idxStart < 0) || (idxStart > length(in.time))) 8 | error('Could not remove data: Incorrect start index'); 9 | end 10 | if ((idxEnd < 0) || (idxEnd > length(in.time)) || (idxEnd < idxStart)) 11 | error('Could not remove data: Incorrect end index'); 12 | end 13 | 14 | out = in; 15 | f = fieldnames(out); 16 | for i = 1:length(f) 17 | out.(f{i})(idxStart:idxEnd,:) = []; 18 | end 19 | 20 | end -------------------------------------------------------------------------------- /DataProcessing/functions/Trim.m: -------------------------------------------------------------------------------- 1 | function out = Trim(in, timeStart, varargin) 2 | if (length(varargin) > 0) 3 | extractionLength = varargin{1}; 4 | else 5 | extractionLength = inf; 6 | end 7 | 8 | % trim output data 9 | idxStart = find(in.time >= timeStart); idxStart = idxStart(1); 10 | idxEnd = find(in.time <= timeStart+extractionLength); idxEnd = idxEnd(end); 11 | 12 | if ((idxStart < 0) || (idxStart > length(in.time))) 13 | error('Could not trim data: Incorrect start index'); 14 | end 15 | if ((idxEnd < 0) || (idxEnd > length(in.time)) || (idxEnd < idxStart)) 16 | error('Could not trim data: Incorrect end index'); 17 | end 18 | 19 | out = []; 20 | f = fieldnames(in); 21 | for i = 1:length(f) 22 | out.(f{i}) = in.(f{i})(idxStart:idxEnd,:); 23 | end 24 | 25 | out.time = out.time - out.time(1); % correct output time to match trimmed away time 26 | 27 | end -------------------------------------------------------------------------------- /DataProcessing/functions/getlatestfile.m: -------------------------------------------------------------------------------- 1 | function latestfile = getlatestfile(directory) 2 | %This function returns the latest file from the directory passsed as input 3 | %argument 4 | 5 | %Get the directory contents 6 | dirc = dir(directory); 7 | 8 | fileListArray = cellfun(@(c)[directory c], {dirc(:).name}, 'uni',false); 9 | 10 | %Filter out all the folders. 11 | dirc = dirc(find(~cellfun(@isdir,fileListArray))); 12 | 13 | %I contains the index to the biggest number which is the latest file 14 | [A,I] = max([dirc(:).datenum]); 15 | 16 | if ~isempty(I) 17 | latestfile = dirc(I).name; 18 | end 19 | 20 | end 21 | -------------------------------------------------------------------------------- /Estimators/Madgwick/ExampleScript.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mindThomas/Kugle-MATLAB/b1ee16de78899a131241639d620b5fb7c02a586b/Estimators/Madgwick/ExampleScript.m -------------------------------------------------------------------------------- /Estimators/Madgwick/LICENSE: -------------------------------------------------------------------------------- 1 | GNU General Public Licence 2 | 3 | Copyright (C) 2009 Sebastian Madgwick. All rights reserved. 4 | ------------------------------------------------------------------------------- 5 | Web : http://x-io.co.uk/open-source-imu-and-ahrs-algorithms/ 6 | ------------------------------------------------------------------------------- 7 | 8 | Permission is hereby granted, free of charge, to any person obtaining a copy 9 | of this software and associated documentation files (the "Software"), to deal 10 | in the Software without restriction, including without limitation the rights 11 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 12 | copies of the Software, and to permit persons to whom the Software is 13 | furnished to do so, subject to the following conditions: 14 | 15 | The above copyright notice and this permission notice shall be included in all 16 | copies or substantial portions of the Software. 17 | 18 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 19 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 20 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 21 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 22 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 23 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 24 | SOFTWARE. 25 | 26 | 27 | -------------------------------------------------------------------------------- /Estimators/Madgwick/quaternion_library/axisAngle2quatern.m: -------------------------------------------------------------------------------- 1 | function q = axisAngle2quatern(axis, angle) 2 | %AXISANGLE2QUATERN Converts an axis-angle orientation to a quaternion 3 | % 4 | % q = axisAngle2quatern(axis, angle) 5 | % 6 | % Converts and axis-angle orientation to a quaternion where a 3D rotation 7 | % is described by an angular rotation around axis defined by a vector. 8 | % 9 | % For more information see: 10 | % http://www.x-io.co.uk/node/8#quaternions 11 | % 12 | % Date Author Notes 13 | % 27/09/2011 SOH Madgwick Initial release 14 | 15 | q0 = cos(angle./2); 16 | q1 = -axis(:,1)*sin(angle./2); 17 | q2 = -axis(:,2)*sin(angle./2); 18 | q3 = -axis(:,3)*sin(angle./2); 19 | q = [q0 q1 q2 q3]; 20 | end 21 | 22 | -------------------------------------------------------------------------------- /Estimators/Madgwick/quaternion_library/axisAngle2rotMat.m: -------------------------------------------------------------------------------- 1 | function R = axisAngle2rotMat(axis, angle) 2 | %AXISANGLE2ROTMAT Converts an axis-angle orientation to a rotation matrix 3 | % 4 | % q = axisAngle2rotMat(axis, angle) 5 | % 6 | % Converts and axis-angle orientation to a rotation matrix where a 3D 7 | % rotation is described by an angular rotation around axis defined by a 8 | % vector. 9 | % 10 | % For more information see: 11 | % http://www.x-io.co.uk/node/8#quaternions 12 | % 13 | % Date Author Notes 14 | % 27/09/2011 SOH Madgwick Initial release 15 | 16 | kx = axis(:,1); 17 | ky = axis(:,2); 18 | kz = axis(:,3); 19 | cT = cos(angle); 20 | sT = sin(angle); 21 | vT = 1 - cos(angle); 22 | 23 | R(1,1,:) = kx.*kx.*vT + cT; 24 | R(1,2,:) = kx.*ky.*vT - kz.*sT; 25 | R(1,3,:) = kx.*kz.*vT + ky.*sT; 26 | 27 | R(2,1,:) = kx.*ky.*vT + kz.*sT; 28 | R(2,2,:) = ky.*ky.*vT + cT; 29 | R(2,3,:) = ky.*kz.*vT - kx.*sT; 30 | 31 | R(3,1,:) = kx.*kz.*vT - ky.*sT; 32 | R(3,2,:) = ky.*kz.*vT + kx.*sT; 33 | R(3,3,:) = kz.*kz.*vT + cT; 34 | end 35 | 36 | -------------------------------------------------------------------------------- /Estimators/Madgwick/quaternion_library/euler2rotMat.m: -------------------------------------------------------------------------------- 1 | function R = euler2rotMat(phi, theta, psi) 2 | %EULER2ROTMAT Converts a ZYX Euler angle orientation to a rotation matrix 3 | % 4 | % q = euler2rotMat(axis, angle) 5 | % 6 | % Converts ZYX Euler angle orientation to a rotation matrix where phi is 7 | % a rotation around X, theta around Y and psi around Z. 8 | % 9 | % For more information see: 10 | % http://www.x-io.co.uk/node/8#quaternions 11 | % 12 | % Date Author Notes 13 | % 27/09/2011 SOH Madgwick Initial release 14 | 15 | R(1,1,:) = cos(psi).*cos(theta); 16 | R(1,2,:) = -sin(psi).*cos(phi) + cos(psi).*sin(theta).*sin(phi); 17 | R(1,3,:) = sin(psi).*sin(phi) + cos(psi).*sin(theta).*cos(phi); 18 | 19 | R(2,1,:) = sin(psi).*cos(theta); 20 | R(2,2,:) = cos(psi).*cos(phi) + sin(psi).*sin(theta).*sin(phi); 21 | R(2,3,:) = -cos(psi).*sin(phi) + sin(psi).*sin(theta).*cos(phi); 22 | 23 | R(3,1,:) = -sin(theta); 24 | R(3,2,:) = cos(theta).*sin(phi); 25 | R(3,3,:) = cos(theta).*cos(phi); 26 | end 27 | 28 | -------------------------------------------------------------------------------- /Estimators/Madgwick/quaternion_library/quatern2euler.m: -------------------------------------------------------------------------------- 1 | function euler = quatern2euler(q) 2 | %QUATERN2EULER Converts a quaternion orientation to ZYX Euler angles 3 | % 4 | % q = quatern2euler(q) 5 | % 6 | % Converts a quaternion orientation to ZYX Euler angles where phi is a 7 | % rotation around X, theta around Y and psi around Z. 8 | % 9 | % For more information see: 10 | % http://www.x-io.co.uk/node/8#quaternions 11 | % 12 | % Date Author Notes 13 | % 27/09/2011 SOH Madgwick Initial release 14 | 15 | R(1,1,:) = 2.*q(:,1).^2-1+2.*q(:,2).^2; 16 | R(2,1,:) = 2.*(q(:,2).*q(:,3)-q(:,1).*q(:,4)); 17 | R(3,1,:) = 2.*(q(:,2).*q(:,4)+q(:,1).*q(:,3)); 18 | R(3,2,:) = 2.*(q(:,3).*q(:,4)-q(:,1).*q(:,2)); 19 | R(3,3,:) = 2.*q(:,1).^2-1+2.*q(:,4).^2; 20 | 21 | phi = atan2(R(3,2,:), R(3,3,:) ); 22 | theta = -atan(R(3,1,:) ./ sqrt(1-R(3,1,:).^2) ); 23 | psi = atan2(R(2,1,:), R(1,1,:) ); 24 | 25 | euler = [phi(1,:)' theta(1,:)' psi(1,:)']; 26 | end 27 | 28 | -------------------------------------------------------------------------------- /Estimators/Madgwick/quaternion_library/quatern2rotMat.m: -------------------------------------------------------------------------------- 1 | function R = quatern2rotMat(q) 2 | %QUATERN2ROTMAT Converts a quaternion orientation to a rotation matrix 3 | % 4 | % R = quatern2rotMat(q) 5 | % 6 | % Converts a quaternion orientation to a rotation matrix. 7 | % 8 | % For more information see: 9 | % http://www.x-io.co.uk/node/8#quaternions 10 | % 11 | % Date Author Notes 12 | % 27/09/2011 SOH Madgwick Initial release 13 | 14 | R(1,1,:) = 2.*q(:,1).^2-1+2.*q(:,2).^2; 15 | R(1,2,:) = 2.*(q(:,2).*q(:,3)+q(:,1).*q(:,4)); 16 | R(1,3,:) = 2.*(q(:,2).*q(:,4)-q(:,1).*q(:,3)); 17 | R(2,1,:) = 2.*(q(:,2).*q(:,3)-q(:,1).*q(:,4)); 18 | R(2,2,:) = 2.*q(:,1).^2-1+2.*q(:,3).^2; 19 | R(2,3,:) = 2.*(q(:,3).*q(:,4)+q(:,1).*q(:,2)); 20 | R(3,1,:) = 2.*(q(:,2).*q(:,4)+q(:,1).*q(:,3)); 21 | R(3,2,:) = 2.*(q(:,3).*q(:,4)-q(:,1).*q(:,2)); 22 | R(3,3,:) = 2.*q(:,1).^2-1+2.*q(:,4).^2; 23 | end 24 | 25 | -------------------------------------------------------------------------------- /Estimators/Madgwick/quaternion_library/quaternConj.m: -------------------------------------------------------------------------------- 1 | function qConj = quaternConj(q) 2 | %QUATERN2ROTMAT Converts a quaternion to its conjugate 3 | % 4 | % qConj = quaternConj(q) 5 | % 6 | % Converts a quaternion to its conjugate. 7 | % 8 | % For more information see: 9 | % http://www.x-io.co.uk/node/8#quaternions 10 | % 11 | % Date Author Notes 12 | % 27/09/2011 SOH Madgwick Initial release 13 | 14 | qConj = [q(:,1) -q(:,2) -q(:,3) -q(:,4)]; 15 | end 16 | -------------------------------------------------------------------------------- /Estimators/Madgwick/quaternion_library/quaternProd.m: -------------------------------------------------------------------------------- 1 | function ab = quaternProd(a, b) 2 | %QUATERNPROD Calculates the quaternion product 3 | % 4 | % ab = quaternProd(a, b) 5 | % 6 | % Calculates the quaternion product of quaternion a and b. 7 | % 8 | % For more information see: 9 | % http://www.x-io.co.uk/node/8#quaternions 10 | % 11 | % Date Author Notes 12 | % 27/09/2011 SOH Madgwick Initial release 13 | 14 | ab(:,1) = a(:,1).*b(:,1)-a(:,2).*b(:,2)-a(:,3).*b(:,3)-a(:,4).*b(:,4); 15 | ab(:,2) = a(:,1).*b(:,2)+a(:,2).*b(:,1)+a(:,3).*b(:,4)-a(:,4).*b(:,3); 16 | ab(:,3) = a(:,1).*b(:,3)-a(:,2).*b(:,4)+a(:,3).*b(:,1)+a(:,4).*b(:,2); 17 | ab(:,4) = a(:,1).*b(:,4)+a(:,2).*b(:,3)-a(:,3).*b(:,2)+a(:,4).*b(:,1); 18 | end 19 | 20 | -------------------------------------------------------------------------------- /Estimators/Madgwick/quaternion_library/rotMat2euler.m: -------------------------------------------------------------------------------- 1 | function euler = rotMat2euler(R) 2 | %ROTMAT2EULER Converts a rotation matrix orientation to ZYX Euler angles 3 | % 4 | % euler = rotMat2euler(R) 5 | % 6 | % Converts a rotation matrix orientation to ZYX Euler angles where phi is 7 | % a rotation around X, theta around Y and psi around Z. 8 | % 9 | % For more information see: 10 | % http://www.x-io.co.uk/node/8#quaternions 11 | % 12 | % Date Author Notes 13 | % 27/09/2011 SOH Madgwick Initial release 14 | 15 | phi = atan2(R(3,2,:), R(3,3,:) ); 16 | theta = -atan(R(3,1,:) ./ sqrt(1-R(3,1,:).^2) ); 17 | psi = atan2(R(2,1,:), R(1,1,:) ); 18 | 19 | euler = [phi(1,:)' theta(1,:)' psi(1,:)']; 20 | end 21 | 22 | -------------------------------------------------------------------------------- /Estimators/QEKF/codegen/lib/QEKF/QEKF_initialize.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Academic License - for use in teaching, academic research, and meeting 3 | // course requirements at degree granting institutions only. Not for 4 | // government, commercial, or other organizational use. 5 | // File: QEKF_initialize.cpp 6 | // 7 | // MATLAB Coder version : 4.0 8 | // C/C++ source code generated on : 26-Apr-2019 13:33:55 9 | // 10 | 11 | // Include Files 12 | #include "rt_nonfinite.h" 13 | #include "QEKF.h" 14 | #include "QEKF_initialize.h" 15 | 16 | // Function Definitions 17 | 18 | // 19 | // Arguments : void 20 | // Return Type : void 21 | // 22 | void QEKF_initialize() 23 | { 24 | rt_InitInfAndNaN(8U); 25 | acc_norm_old_not_empty_init(); 26 | acc_norm_filtered_not_empty_init(); 27 | } 28 | 29 | // 30 | // File trailer for QEKF_initialize.cpp 31 | // 32 | // [EOF] 33 | // 34 | -------------------------------------------------------------------------------- /Estimators/QEKF/codegen/lib/QEKF/QEKF_initialize.h: -------------------------------------------------------------------------------- 1 | // 2 | // Academic License - for use in teaching, academic research, and meeting 3 | // course requirements at degree granting institutions only. Not for 4 | // government, commercial, or other organizational use. 5 | // File: QEKF_initialize.h 6 | // 7 | // MATLAB Coder version : 4.0 8 | // C/C++ source code generated on : 26-Apr-2019 13:33:55 9 | // 10 | #ifndef QEKF_INITIALIZE_H 11 | #define QEKF_INITIALIZE_H 12 | 13 | // Include Files 14 | #include 15 | #include 16 | #include "rtwtypes.h" 17 | #include "QEKF_types.h" 18 | 19 | // Function Declarations 20 | extern void QEKF_initialize(); 21 | 22 | #endif 23 | 24 | // 25 | // File trailer for QEKF_initialize.h 26 | // 27 | // [EOF] 28 | // 29 | -------------------------------------------------------------------------------- /Estimators/QEKF/codegen/lib/QEKF/QEKF_ref.rsp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mindThomas/Kugle-MATLAB/b1ee16de78899a131241639d620b5fb7c02a586b/Estimators/QEKF/codegen/lib/QEKF/QEKF_ref.rsp -------------------------------------------------------------------------------- /Estimators/QEKF/codegen/lib/QEKF/QEKF_rtw.bat: -------------------------------------------------------------------------------- 1 | @echo off 2 | 3 | cd . 4 | 5 | if "%1"=="" ("C:\PROGRA~1\MATLAB\R2018a\bin\win64\gmake" -f QEKF_rtw.mk all) else ("C:\PROGRA~1\MATLAB\R2018a\bin\win64\gmake" -f QEKF_rtw.mk %1) 6 | @if errorlevel 1 goto error_exit 7 | 8 | exit /B 0 9 | 10 | :error_exit 11 | echo The make command returned an error of %errorlevel% 12 | An_error_occurred_during_the_call_to_make 13 | -------------------------------------------------------------------------------- /Estimators/QEKF/codegen/lib/QEKF/QEKF_terminate.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Academic License - for use in teaching, academic research, and meeting 3 | // course requirements at degree granting institutions only. Not for 4 | // government, commercial, or other organizational use. 5 | // File: QEKF_terminate.cpp 6 | // 7 | // MATLAB Coder version : 4.0 8 | // C/C++ source code generated on : 26-Apr-2019 13:33:55 9 | // 10 | 11 | // Include Files 12 | #include "rt_nonfinite.h" 13 | #include "QEKF.h" 14 | #include "QEKF_terminate.h" 15 | 16 | // Function Definitions 17 | 18 | // 19 | // Arguments : void 20 | // Return Type : void 21 | // 22 | void QEKF_terminate() 23 | { 24 | // (no terminate code required) 25 | } 26 | 27 | // 28 | // File trailer for QEKF_terminate.cpp 29 | // 30 | // [EOF] 31 | // 32 | -------------------------------------------------------------------------------- /Estimators/QEKF/codegen/lib/QEKF/QEKF_terminate.h: -------------------------------------------------------------------------------- 1 | // 2 | // Academic License - for use in teaching, academic research, and meeting 3 | // course requirements at degree granting institutions only. Not for 4 | // government, commercial, or other organizational use. 5 | // File: QEKF_terminate.h 6 | // 7 | // MATLAB Coder version : 4.0 8 | // C/C++ source code generated on : 26-Apr-2019 13:33:55 9 | // 10 | #ifndef QEKF_TERMINATE_H 11 | #define QEKF_TERMINATE_H 12 | 13 | // Include Files 14 | #include 15 | #include 16 | #include "rtwtypes.h" 17 | #include "QEKF_types.h" 18 | 19 | // Function Declarations 20 | extern void QEKF_terminate(); 21 | 22 | #endif 23 | 24 | // 25 | // File trailer for QEKF_terminate.h 26 | // 27 | // [EOF] 28 | // 29 | -------------------------------------------------------------------------------- /Estimators/QEKF/codegen/lib/QEKF/QEKF_types.h: -------------------------------------------------------------------------------- 1 | // 2 | // Academic License - for use in teaching, academic research, and meeting 3 | // course requirements at degree granting institutions only. Not for 4 | // government, commercial, or other organizational use. 5 | // File: QEKF_types.h 6 | // 7 | // MATLAB Coder version : 4.0 8 | // C/C++ source code generated on : 26-Apr-2019 13:33:55 9 | // 10 | #ifndef QEKF_TYPES_H 11 | #define QEKF_TYPES_H 12 | 13 | // Include Files 14 | #include "rtwtypes.h" 15 | #endif 16 | 17 | // 18 | // File trailer for QEKF_types.h 19 | // 20 | // [EOF] 21 | // 22 | -------------------------------------------------------------------------------- /Estimators/QEKF/codegen/lib/QEKF/buildInfo.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mindThomas/Kugle-MATLAB/b1ee16de78899a131241639d620b5fb7c02a586b/Estimators/QEKF/codegen/lib/QEKF/buildInfo.mat -------------------------------------------------------------------------------- /Estimators/QEKF/codegen/lib/QEKF/codeInfo.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mindThomas/Kugle-MATLAB/b1ee16de78899a131241639d620b5fb7c02a586b/Estimators/QEKF/codegen/lib/QEKF/codeInfo.mat -------------------------------------------------------------------------------- /Estimators/QEKF/codegen/lib/QEKF/codedescriptor.dmr: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mindThomas/Kugle-MATLAB/b1ee16de78899a131241639d620b5fb7c02a586b/Estimators/QEKF/codegen/lib/QEKF/codedescriptor.dmr -------------------------------------------------------------------------------- /Estimators/QEKF/codegen/lib/QEKF/eye.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Academic License - for use in teaching, academic research, and meeting 3 | // course requirements at degree granting institutions only. Not for 4 | // government, commercial, or other organizational use. 5 | // File: eye.cpp 6 | // 7 | // MATLAB Coder version : 4.0 8 | // C/C++ source code generated on : 26-Apr-2019 13:33:55 9 | // 10 | 11 | // Include Files 12 | #include 13 | #include "rt_nonfinite.h" 14 | #include "QEKF.h" 15 | #include "eye.h" 16 | 17 | // Function Definitions 18 | 19 | // 20 | // Arguments : double I[16] 21 | // Return Type : void 22 | // 23 | void b_eye(double I[16]) 24 | { 25 | int k; 26 | memset(&I[0], 0, sizeof(double) << 4); 27 | for (k = 0; k < 4; k++) { 28 | I[k + (k << 2)] = 1.0; 29 | } 30 | } 31 | 32 | // 33 | // Arguments : double I[100] 34 | // Return Type : void 35 | // 36 | void c_eye(double I[100]) 37 | { 38 | int k; 39 | memset(&I[0], 0, 100U * sizeof(double)); 40 | for (k = 0; k < 10; k++) { 41 | I[k + 10 * k] = 1.0; 42 | } 43 | } 44 | 45 | // 46 | // Arguments : double I[9] 47 | // Return Type : void 48 | // 49 | void eye(double I[9]) 50 | { 51 | int k; 52 | memset(&I[0], 0, 9U * sizeof(double)); 53 | for (k = 0; k < 3; k++) { 54 | I[k + 3 * k] = 1.0; 55 | } 56 | } 57 | 58 | // 59 | // File trailer for eye.cpp 60 | // 61 | // [EOF] 62 | // 63 | -------------------------------------------------------------------------------- /Estimators/QEKF/codegen/lib/QEKF/eye.h: -------------------------------------------------------------------------------- 1 | // 2 | // Academic License - for use in teaching, academic research, and meeting 3 | // course requirements at degree granting institutions only. Not for 4 | // government, commercial, or other organizational use. 5 | // File: eye.h 6 | // 7 | // MATLAB Coder version : 4.0 8 | // C/C++ source code generated on : 26-Apr-2019 13:33:55 9 | // 10 | #ifndef EYE_H 11 | #define EYE_H 12 | 13 | // Include Files 14 | #include 15 | #include 16 | #include "rtwtypes.h" 17 | #include "QEKF_types.h" 18 | 19 | // Function Declarations 20 | extern void b_eye(double I[16]); 21 | extern void c_eye(double I[100]); 22 | extern void eye(double I[9]); 23 | 24 | #endif 25 | 26 | // 27 | // File trailer for eye.h 28 | // 29 | // [EOF] 30 | // 31 | -------------------------------------------------------------------------------- /Estimators/QEKF/codegen/lib/QEKF/html/report.mldatx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mindThomas/Kugle-MATLAB/b1ee16de78899a131241639d620b5fb7c02a586b/Estimators/QEKF/codegen/lib/QEKF/html/report.mldatx -------------------------------------------------------------------------------- /Estimators/QEKF/codegen/lib/QEKF/interface/_coder_QEKF_info.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Academic License - for use in teaching, academic research, and meeting 3 | * course requirements at degree granting institutions only. Not for 4 | * government, commercial, or other organizational use. 5 | * File: _coder_QEKF_info.h 6 | * 7 | * MATLAB Coder version : 4.0 8 | * C/C++ source code generated on : 26-Apr-2019 13:33:55 9 | */ 10 | 11 | #ifndef _CODER_QEKF_INFO_H 12 | #define _CODER_QEKF_INFO_H 13 | /* Include Files */ 14 | #include "tmwtypes.h" 15 | #include "mex.h" 16 | #include "emlrt.h" 17 | 18 | 19 | /* Function Declarations */ 20 | extern const mxArray *emlrtMexFcnResolvedFunctionsInfo(); 21 | MEXFUNCTION_LINKAGE mxArray *emlrtMexFcnProperties(); 22 | 23 | #endif 24 | /* 25 | * File trailer for _coder_QEKF_info.h 26 | * 27 | * [EOF] 28 | */ 29 | -------------------------------------------------------------------------------- /Estimators/QEKF/codegen/lib/QEKF/interface/_coder_QEKF_mex.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Academic License - for use in teaching, academic research, and meeting 3 | * course requirements at degree granting institutions only. Not for 4 | * government, commercial, or other organizational use. 5 | * File: _coder_QEKF_mex.h 6 | * 7 | * MATLAB Coder version : 4.0 8 | * C/C++ source code generated on : 26-Apr-2019 13:33:55 9 | */ 10 | 11 | #ifndef _CODER_QEKF_MEX_H 12 | #define _CODER_QEKF_MEX_H 13 | 14 | /* Include Files */ 15 | #include 16 | #include 17 | #include 18 | #include "tmwtypes.h" 19 | #include "mex.h" 20 | #include "emlrt.h" 21 | #include "_coder_QEKF_api.h" 22 | 23 | /* Function Declarations */ 24 | extern void mexFunction(int32_T nlhs, mxArray *plhs[], int32_T nrhs, const 25 | mxArray *prhs[]); 26 | extern emlrtCTX mexFunctionCreateRootTLS(void); 27 | 28 | #endif 29 | 30 | /* 31 | * File trailer for _coder_QEKF_mex.h 32 | * 33 | * [EOF] 34 | */ 35 | -------------------------------------------------------------------------------- /Estimators/QEKF/codegen/lib/QEKF/mrdivide.h: -------------------------------------------------------------------------------- 1 | // 2 | // Academic License - for use in teaching, academic research, and meeting 3 | // course requirements at degree granting institutions only. Not for 4 | // government, commercial, or other organizational use. 5 | // File: mrdivide.h 6 | // 7 | // MATLAB Coder version : 4.0 8 | // C/C++ source code generated on : 26-Apr-2019 13:33:55 9 | // 10 | #ifndef MRDIVIDE_H 11 | #define MRDIVIDE_H 12 | 13 | // Include Files 14 | #include 15 | #include 16 | #include "rtwtypes.h" 17 | #include "QEKF_types.h" 18 | 19 | // Function Declarations 20 | extern void b_mrdivide(const float A[60], const float B[36], float y[60]); 21 | extern void mrdivide(const float A[70], const float B[49], float y[70]); 22 | 23 | #endif 24 | 25 | // 26 | // File trailer for mrdivide.h 27 | // 28 | // [EOF] 29 | // 30 | -------------------------------------------------------------------------------- /Estimators/QEKF/codegen/lib/QEKF/norm.h: -------------------------------------------------------------------------------- 1 | // 2 | // Academic License - for use in teaching, academic research, and meeting 3 | // course requirements at degree granting institutions only. Not for 4 | // government, commercial, or other organizational use. 5 | // File: norm.h 6 | // 7 | // MATLAB Coder version : 4.0 8 | // C/C++ source code generated on : 26-Apr-2019 13:33:55 9 | // 10 | #ifndef NORM_H 11 | #define NORM_H 12 | 13 | // Include Files 14 | #include 15 | #include 16 | #include "rtwtypes.h" 17 | #include "QEKF_types.h" 18 | 19 | // Function Declarations 20 | extern float b_norm(const float x[4]); 21 | extern float norm(const float x[3]); 22 | 23 | #endif 24 | 25 | // 26 | // File trailer for norm.h 27 | // 28 | // [EOF] 29 | // 30 | -------------------------------------------------------------------------------- /Estimators/QEKF/codegen/lib/QEKF/rtGetInf.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Academic License - for use in teaching, academic research, and meeting 3 | * course requirements at degree granting institutions only. Not for 4 | * government, commercial, or other organizational use. 5 | * File: rtGetInf.h 6 | * 7 | * MATLAB Coder version : 4.0 8 | * C/C++ source code generated on : 26-Apr-2019 13:33:55 9 | */ 10 | 11 | #ifndef RTGETINF_H 12 | #define RTGETINF_H 13 | #include 14 | #include "rtwtypes.h" 15 | #include "rt_nonfinite.h" 16 | 17 | extern real_T rtGetInf(void); 18 | extern real32_T rtGetInfF(void); 19 | extern real_T rtGetMinusInf(void); 20 | extern real32_T rtGetMinusInfF(void); 21 | 22 | #endif 23 | 24 | /* 25 | * File trailer for rtGetInf.h 26 | * 27 | * [EOF] 28 | */ 29 | -------------------------------------------------------------------------------- /Estimators/QEKF/codegen/lib/QEKF/rtGetNaN.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Academic License - for use in teaching, academic research, and meeting 3 | * course requirements at degree granting institutions only. Not for 4 | * government, commercial, or other organizational use. 5 | * File: rtGetNaN.h 6 | * 7 | * MATLAB Coder version : 4.0 8 | * C/C++ source code generated on : 26-Apr-2019 13:33:55 9 | */ 10 | 11 | #ifndef RTGETNAN_H 12 | #define RTGETNAN_H 13 | #include 14 | #include "rtwtypes.h" 15 | #include "rt_nonfinite.h" 16 | 17 | extern real_T rtGetNaN(void); 18 | extern real32_T rtGetNaNF(void); 19 | 20 | #endif 21 | 22 | /* 23 | * File trailer for rtGetNaN.h 24 | * 25 | * [EOF] 26 | */ 27 | -------------------------------------------------------------------------------- /Estimators/QEKF/codegen/lib/QEKF/rt_defines.h: -------------------------------------------------------------------------------- 1 | // 2 | // Academic License - for use in teaching, academic research, and meeting 3 | // course requirements at degree granting institutions only. Not for 4 | // government, commercial, or other organizational use. 5 | // File: rt_defines.h 6 | // 7 | // MATLAB Coder version : 4.0 8 | // C/C++ source code generated on : 26-Apr-2019 13:33:55 9 | // 10 | #ifndef RT_DEFINES_H 11 | #define RT_DEFINES_H 12 | #include 13 | #define RT_PI 3.14159265358979323846 14 | #define RT_PIF 3.1415927F 15 | #define RT_LN_10 2.30258509299404568402 16 | #define RT_LN_10F 2.3025851F 17 | #define RT_LOG10E 0.43429448190325182765 18 | #define RT_LOG10EF 0.43429449F 19 | #define RT_E 2.7182818284590452354 20 | #define RT_EF 2.7182817F 21 | #endif 22 | 23 | // 24 | // File trailer for rt_defines.h 25 | // 26 | // [EOF] 27 | // 28 | -------------------------------------------------------------------------------- /Estimators/QEKF/codegen/lib/QEKF/rtw_proj.tmw: -------------------------------------------------------------------------------- 1 | Code generation project for QEKF using toolchain "GNU Tools for ARM Embedded Processors v5.2 | gmake (64-bit Windows)". MATLAB root = C:\Program Files\MATLAB\R2018a. 2 | -------------------------------------------------------------------------------- /Estimators/VEKF/Codegen.m: -------------------------------------------------------------------------------- 1 | clear all; 2 | scriptDir = fileparts(mfilename('fullpath')); 3 | addpath(fullfile(scriptDir, '../../Codegen')); 4 | load('CoderConfig_ARM_CortexM.mat'); 5 | 6 | % Define input sizes and types 7 | X = coder.typeof(single(0), [7 1], 0); % fixed size of floats 8 | P_prev = coder.typeof(single(0), [7 7], 0); 9 | EncoderDiffMeas = coder.typeof(single(0), [3 1], 0); 10 | eta_encoder = coder.typeof(single(0)); 11 | Accelerometer = coder.typeof(single(0), [3 1], 0); 12 | cov_acc = coder.typeof(single(0), [3 3], 0); 13 | eta_accelerometer = coder.typeof(single(0)); 14 | eta_bias = coder.typeof(single(0)); 15 | qQEKF = coder.typeof(single(0), [4 1], 0); 16 | cov_qQEKF = coder.typeof(single(0), [4 4], 0); 17 | qdotQEKF = coder.typeof(single(0), [4 1], 0); 18 | eta_acceleration = coder.typeof(single(0)); 19 | SamplePeriod = coder.typeof(single(0)); 20 | TicksPrRev = coder.typeof(single(0)); 21 | rk = coder.typeof(single(0)); 22 | rw = coder.typeof(single(0)); 23 | g = coder.typeof(single(0)); 24 | 25 | % Convert QEKF to C++ code 26 | codegen VelocityEKF -config config -nargout 2 -args { X, P_prev, EncoderDiffMeas, eta_encoder, Accelerometer, cov_acc, eta_accelerometer, eta_bias, qQEKF, cov_qQEKF, qdotQEKF, eta_acceleration, SamplePeriod, TicksPrRev, rk,rw,g } -------------------------------------------------------------------------------- /Estimators/VEKF/codegen/lib/VelocityEKF/VelocityEKF.h: -------------------------------------------------------------------------------- 1 | // 2 | // Academic License - for use in teaching, academic research, and meeting 3 | // course requirements at degree granting institutions only. Not for 4 | // government, commercial, or other organizational use. 5 | // File: VelocityEKF.h 6 | // 7 | // MATLAB Coder version : 4.0 8 | // C/C++ source code generated on : 26-Apr-2019 13:40:20 9 | // 10 | #ifndef VELOCITYEKF_H 11 | #define VELOCITYEKF_H 12 | 13 | // Include Files 14 | #include 15 | #include 16 | #include "rtwtypes.h" 17 | #include "VelocityEKF_types.h" 18 | 19 | // Function Declarations 20 | extern void VelocityEKF(const float X[7], const float P_prev[49], const float 21 | EncoderDiffMeas[3], float eta_encoder, const float Accelerometer[3], const 22 | float cov_acc[9], float eta_accelerometer, float eta_bias, const float qQEKF[4], 23 | const float cov_qQEKF[16], const float qdotQEKF[4], float eta_acceleration, 24 | float SamplePeriod, float TicksPrRev, float rk, float rw, float g, float 25 | X_out[7], float P_out[49]); 26 | 27 | #endif 28 | 29 | // 30 | // File trailer for VelocityEKF.h 31 | // 32 | // [EOF] 33 | // 34 | -------------------------------------------------------------------------------- /Estimators/VEKF/codegen/lib/VelocityEKF/VelocityEKF_initialize.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Academic License - for use in teaching, academic research, and meeting 3 | // course requirements at degree granting institutions only. Not for 4 | // government, commercial, or other organizational use. 5 | // File: VelocityEKF_initialize.cpp 6 | // 7 | // MATLAB Coder version : 4.0 8 | // C/C++ source code generated on : 26-Apr-2019 13:40:20 9 | // 10 | 11 | // Include Files 12 | #include "rt_nonfinite.h" 13 | #include "VelocityEKF.h" 14 | #include "VelocityEKF_initialize.h" 15 | 16 | // Function Definitions 17 | 18 | // 19 | // Arguments : void 20 | // Return Type : void 21 | // 22 | void VelocityEKF_initialize() 23 | { 24 | rt_InitInfAndNaN(8U); 25 | } 26 | 27 | // 28 | // File trailer for VelocityEKF_initialize.cpp 29 | // 30 | // [EOF] 31 | // 32 | -------------------------------------------------------------------------------- /Estimators/VEKF/codegen/lib/VelocityEKF/VelocityEKF_initialize.h: -------------------------------------------------------------------------------- 1 | // 2 | // Academic License - for use in teaching, academic research, and meeting 3 | // course requirements at degree granting institutions only. Not for 4 | // government, commercial, or other organizational use. 5 | // File: VelocityEKF_initialize.h 6 | // 7 | // MATLAB Coder version : 4.0 8 | // C/C++ source code generated on : 26-Apr-2019 13:40:20 9 | // 10 | #ifndef VELOCITYEKF_INITIALIZE_H 11 | #define VELOCITYEKF_INITIALIZE_H 12 | 13 | // Include Files 14 | #include 15 | #include 16 | #include "rtwtypes.h" 17 | #include "VelocityEKF_types.h" 18 | 19 | // Function Declarations 20 | extern void VelocityEKF_initialize(); 21 | 22 | #endif 23 | 24 | // 25 | // File trailer for VelocityEKF_initialize.h 26 | // 27 | // [EOF] 28 | // 29 | -------------------------------------------------------------------------------- /Estimators/VEKF/codegen/lib/VelocityEKF/VelocityEKF_ref.rsp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mindThomas/Kugle-MATLAB/b1ee16de78899a131241639d620b5fb7c02a586b/Estimators/VEKF/codegen/lib/VelocityEKF/VelocityEKF_ref.rsp -------------------------------------------------------------------------------- /Estimators/VEKF/codegen/lib/VelocityEKF/VelocityEKF_rtw.bat: -------------------------------------------------------------------------------- 1 | @echo off 2 | 3 | cd . 4 | 5 | if "%1"=="" ("C:\PROGRA~1\MATLAB\R2018a\bin\win64\gmake" -f VelocityEKF_rtw.mk all) else ("C:\PROGRA~1\MATLAB\R2018a\bin\win64\gmake" -f VelocityEKF_rtw.mk %1) 6 | @if errorlevel 1 goto error_exit 7 | 8 | exit /B 0 9 | 10 | :error_exit 11 | echo The make command returned an error of %errorlevel% 12 | An_error_occurred_during_the_call_to_make 13 | -------------------------------------------------------------------------------- /Estimators/VEKF/codegen/lib/VelocityEKF/VelocityEKF_terminate.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Academic License - for use in teaching, academic research, and meeting 3 | // course requirements at degree granting institutions only. Not for 4 | // government, commercial, or other organizational use. 5 | // File: VelocityEKF_terminate.cpp 6 | // 7 | // MATLAB Coder version : 4.0 8 | // C/C++ source code generated on : 26-Apr-2019 13:40:20 9 | // 10 | 11 | // Include Files 12 | #include "rt_nonfinite.h" 13 | #include "VelocityEKF.h" 14 | #include "VelocityEKF_terminate.h" 15 | 16 | // Function Definitions 17 | 18 | // 19 | // Arguments : void 20 | // Return Type : void 21 | // 22 | void VelocityEKF_terminate() 23 | { 24 | // (no terminate code required) 25 | } 26 | 27 | // 28 | // File trailer for VelocityEKF_terminate.cpp 29 | // 30 | // [EOF] 31 | // 32 | -------------------------------------------------------------------------------- /Estimators/VEKF/codegen/lib/VelocityEKF/VelocityEKF_terminate.h: -------------------------------------------------------------------------------- 1 | // 2 | // Academic License - for use in teaching, academic research, and meeting 3 | // course requirements at degree granting institutions only. Not for 4 | // government, commercial, or other organizational use. 5 | // File: VelocityEKF_terminate.h 6 | // 7 | // MATLAB Coder version : 4.0 8 | // C/C++ source code generated on : 26-Apr-2019 13:40:20 9 | // 10 | #ifndef VELOCITYEKF_TERMINATE_H 11 | #define VELOCITYEKF_TERMINATE_H 12 | 13 | // Include Files 14 | #include 15 | #include 16 | #include "rtwtypes.h" 17 | #include "VelocityEKF_types.h" 18 | 19 | // Function Declarations 20 | extern void VelocityEKF_terminate(); 21 | 22 | #endif 23 | 24 | // 25 | // File trailer for VelocityEKF_terminate.h 26 | // 27 | // [EOF] 28 | // 29 | -------------------------------------------------------------------------------- /Estimators/VEKF/codegen/lib/VelocityEKF/VelocityEKF_types.h: -------------------------------------------------------------------------------- 1 | // 2 | // Academic License - for use in teaching, academic research, and meeting 3 | // course requirements at degree granting institutions only. Not for 4 | // government, commercial, or other organizational use. 5 | // File: VelocityEKF_types.h 6 | // 7 | // MATLAB Coder version : 4.0 8 | // C/C++ source code generated on : 26-Apr-2019 13:40:20 9 | // 10 | #ifndef VELOCITYEKF_TYPES_H 11 | #define VELOCITYEKF_TYPES_H 12 | 13 | // Include Files 14 | #include "rtwtypes.h" 15 | #endif 16 | 17 | // 18 | // File trailer for VelocityEKF_types.h 19 | // 20 | // [EOF] 21 | // 22 | -------------------------------------------------------------------------------- /Estimators/VEKF/codegen/lib/VelocityEKF/buildInfo.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mindThomas/Kugle-MATLAB/b1ee16de78899a131241639d620b5fb7c02a586b/Estimators/VEKF/codegen/lib/VelocityEKF/buildInfo.mat -------------------------------------------------------------------------------- /Estimators/VEKF/codegen/lib/VelocityEKF/codeInfo.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mindThomas/Kugle-MATLAB/b1ee16de78899a131241639d620b5fb7c02a586b/Estimators/VEKF/codegen/lib/VelocityEKF/codeInfo.mat -------------------------------------------------------------------------------- /Estimators/VEKF/codegen/lib/VelocityEKF/codedescriptor.dmr: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mindThomas/Kugle-MATLAB/b1ee16de78899a131241639d620b5fb7c02a586b/Estimators/VEKF/codegen/lib/VelocityEKF/codedescriptor.dmr -------------------------------------------------------------------------------- /Estimators/VEKF/codegen/lib/VelocityEKF/html/report.mldatx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mindThomas/Kugle-MATLAB/b1ee16de78899a131241639d620b5fb7c02a586b/Estimators/VEKF/codegen/lib/VelocityEKF/html/report.mldatx -------------------------------------------------------------------------------- /Estimators/VEKF/codegen/lib/VelocityEKF/interface/_coder_VelocityEKF_info.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Academic License - for use in teaching, academic research, and meeting 3 | * course requirements at degree granting institutions only. Not for 4 | * government, commercial, or other organizational use. 5 | * File: _coder_VelocityEKF_info.h 6 | * 7 | * MATLAB Coder version : 4.0 8 | * C/C++ source code generated on : 26-Apr-2019 13:40:20 9 | */ 10 | 11 | #ifndef _CODER_VELOCITYEKF_INFO_H 12 | #define _CODER_VELOCITYEKF_INFO_H 13 | /* Include Files */ 14 | #include "tmwtypes.h" 15 | #include "mex.h" 16 | #include "emlrt.h" 17 | 18 | 19 | /* Function Declarations */ 20 | extern const mxArray *emlrtMexFcnResolvedFunctionsInfo(); 21 | MEXFUNCTION_LINKAGE mxArray *emlrtMexFcnProperties(); 22 | 23 | #endif 24 | /* 25 | * File trailer for _coder_VelocityEKF_info.h 26 | * 27 | * [EOF] 28 | */ 29 | -------------------------------------------------------------------------------- /Estimators/VEKF/codegen/lib/VelocityEKF/interface/_coder_VelocityEKF_mex.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Academic License - for use in teaching, academic research, and meeting 3 | * course requirements at degree granting institutions only. Not for 4 | * government, commercial, or other organizational use. 5 | * File: _coder_VelocityEKF_mex.h 6 | * 7 | * MATLAB Coder version : 4.0 8 | * C/C++ source code generated on : 26-Apr-2019 13:40:20 9 | */ 10 | 11 | #ifndef _CODER_VELOCITYEKF_MEX_H 12 | #define _CODER_VELOCITYEKF_MEX_H 13 | 14 | /* Include Files */ 15 | #include 16 | #include 17 | #include 18 | #include "tmwtypes.h" 19 | #include "mex.h" 20 | #include "emlrt.h" 21 | #include "_coder_VelocityEKF_api.h" 22 | 23 | /* Function Declarations */ 24 | extern void mexFunction(int32_T nlhs, mxArray *plhs[], int32_T nrhs, const 25 | mxArray *prhs[]); 26 | extern emlrtCTX mexFunctionCreateRootTLS(void); 27 | 28 | #endif 29 | 30 | /* 31 | * File trailer for _coder_VelocityEKF_mex.h 32 | * 33 | * [EOF] 34 | */ 35 | -------------------------------------------------------------------------------- /Estimators/VEKF/codegen/lib/VelocityEKF/mrdivide.h: -------------------------------------------------------------------------------- 1 | // 2 | // Academic License - for use in teaching, academic research, and meeting 3 | // course requirements at degree granting institutions only. Not for 4 | // government, commercial, or other organizational use. 5 | // File: mrdivide.h 6 | // 7 | // MATLAB Coder version : 4.0 8 | // C/C++ source code generated on : 26-Apr-2019 13:40:20 9 | // 10 | #ifndef MRDIVIDE_H 11 | #define MRDIVIDE_H 12 | 13 | // Include Files 14 | #include 15 | #include 16 | #include "rtwtypes.h" 17 | #include "VelocityEKF_types.h" 18 | 19 | // Function Declarations 20 | extern void mrdivide(const float A[42], const float B[36], float y[42]); 21 | 22 | #endif 23 | 24 | // 25 | // File trailer for mrdivide.h 26 | // 27 | // [EOF] 28 | // 29 | -------------------------------------------------------------------------------- /Estimators/VEKF/codegen/lib/VelocityEKF/rtGetInf.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Academic License - for use in teaching, academic research, and meeting 3 | * course requirements at degree granting institutions only. Not for 4 | * government, commercial, or other organizational use. 5 | * File: rtGetInf.h 6 | * 7 | * MATLAB Coder version : 4.0 8 | * C/C++ source code generated on : 26-Apr-2019 13:40:20 9 | */ 10 | 11 | #ifndef RTGETINF_H 12 | #define RTGETINF_H 13 | #include 14 | #include "rtwtypes.h" 15 | #include "rt_nonfinite.h" 16 | 17 | extern real_T rtGetInf(void); 18 | extern real32_T rtGetInfF(void); 19 | extern real_T rtGetMinusInf(void); 20 | extern real32_T rtGetMinusInfF(void); 21 | 22 | #endif 23 | 24 | /* 25 | * File trailer for rtGetInf.h 26 | * 27 | * [EOF] 28 | */ 29 | -------------------------------------------------------------------------------- /Estimators/VEKF/codegen/lib/VelocityEKF/rtGetNaN.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Academic License - for use in teaching, academic research, and meeting 3 | * course requirements at degree granting institutions only. Not for 4 | * government, commercial, or other organizational use. 5 | * File: rtGetNaN.h 6 | * 7 | * MATLAB Coder version : 4.0 8 | * C/C++ source code generated on : 26-Apr-2019 13:40:20 9 | */ 10 | 11 | #ifndef RTGETNAN_H 12 | #define RTGETNAN_H 13 | #include 14 | #include "rtwtypes.h" 15 | #include "rt_nonfinite.h" 16 | 17 | extern real_T rtGetNaN(void); 18 | extern real32_T rtGetNaNF(void); 19 | 20 | #endif 21 | 22 | /* 23 | * File trailer for rtGetNaN.h 24 | * 25 | * [EOF] 26 | */ 27 | -------------------------------------------------------------------------------- /Estimators/VEKF/codegen/lib/VelocityEKF/rtw_proj.tmw: -------------------------------------------------------------------------------- 1 | Code generation project for VelocityEKF using toolchain "GNU Tools for ARM Embedded Processors v5.2 | gmake (64-bit Windows)". MATLAB root = C:\Program Files\MATLAB\R2018a. 2 | -------------------------------------------------------------------------------- /Kugle - Modelling and Control of a Ball-balancing Robot.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mindThomas/Kugle-MATLAB/b1ee16de78899a131241639d620b5fb7c02a586b/Kugle - Modelling and Control of a Ball-balancing Robot.pdf -------------------------------------------------------------------------------- /Kugle - Presentation slides.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mindThomas/Kugle-MATLAB/b1ee16de78899a131241639d620b5fb7c02a586b/Kugle - Presentation slides.pdf -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (C) 2018-2019 Thomas Jespersen, TKJ Electronics. All rights reserved. 4 | ------------------------------------------------------------------------------- 5 | Web : http://www.tkjelectronics.dk 6 | e-mail : thomasj@tkjelectronics.dk 7 | ------------------------------------------------------------------------------- 8 | 9 | Permission is hereby granted, free of charge, to any person obtaining a copy 10 | of this software and associated documentation files (the "Software"), to deal 11 | in the Software without restriction, including without limitation the rights 12 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 13 | copies of the Software, and to permit persons to whom the Software is 14 | furnished to do so, subject to the following conditions: 15 | 16 | The above copyright notice and this permission notice shall be included in all 17 | copies or substantial portions of the Software. 18 | 19 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 20 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 21 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 22 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 23 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 24 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 25 | SOFTWARE. 26 | 27 | 28 | -------------------------------------------------------------------------------- /Linearization/generated/ClosedLoopModelMatrices_SlidingMode.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mindThomas/Kugle-MATLAB/b1ee16de78899a131241639d620b5fb7c02a586b/Linearization/generated/ClosedLoopModelMatrices_SlidingMode.mat -------------------------------------------------------------------------------- /Linearization/generated/ClosedLoopModelMatrices_SlidingMode_WithOmegaRef.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mindThomas/Kugle-MATLAB/b1ee16de78899a131241639d620b5fb7c02a586b/Linearization/generated/ClosedLoopModelMatrices_SlidingMode_WithOmegaRef.mat -------------------------------------------------------------------------------- /Linearization/generated/LinearizedModelMatrices.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mindThomas/Kugle-MATLAB/b1ee16de78899a131241639d620b5fb7c02a586b/Linearization/generated/LinearizedModelMatrices.mat -------------------------------------------------------------------------------- /Linearization/generated/SteadyStateAccelerationConstants.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mindThomas/Kugle-MATLAB/b1ee16de78899a131241639d620b5fb7c02a586b/Linearization/generated/SteadyStateAccelerationConstants.mat -------------------------------------------------------------------------------- /MeasurementDataset/PlotVicon.m: -------------------------------------------------------------------------------- 1 | t = ObjData.Time; 2 | q = ObjData.Quaternion; 3 | 4 | flipIdx = [1; (find(abs(diff(q(:,1))) > 0.5)+1); length(t)]; 5 | qfix = q; 6 | setsign = 1; 7 | for (i = 1:(length(flipIdx)-1)) 8 | qfix(flipIdx(i):flipIdx(i+1),:) = setsign * q(flipIdx(i):flipIdx(i+1),:); 9 | setsign = -setsign; 10 | end 11 | q = qfix; 12 | 13 | eul = quat2eul(q, 'ZYX'); 14 | figure(1); 15 | subplot(3,1,1); 16 | plot(t, rad2deg(eul(:,3))); title('Roll'); ylabel('deg'); 17 | subplot(3,1,2); 18 | plot(t, rad2deg(eul(:,2))); title('Pitch'); ylabel('deg'); 19 | subplot(3,1,3); 20 | plot(t, rad2deg(eul(:,1))); title('Yaw'); ylabel('deg'); 21 | 22 | figure(2); 23 | subplot(4,1,1); 24 | plot(t, q(:,1)); 25 | subplot(4,1,2); 26 | plot(t, q(:,2)); 27 | subplot(4,1,3); 28 | plot(t, q(:,3)); 29 | subplot(4,1,4); 30 | plot(t, q(:,4)); 31 | 32 | figure(3); 33 | x = ObjData.XYZ(:,2)'; 34 | y = -ObjData.XYZ(:,1)'; 35 | z = ObjData.XYZ(:,3)'; 36 | surface([x;x],[y;y],[z;z],[t';t'],... 37 | 'facecol','no',... 38 | 'edgecol','interp',... 39 | 'linew',2); 40 | axis equal; -------------------------------------------------------------------------------- /MeasurementDataset/README.md: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mindThomas/Kugle-MATLAB/b1ee16de78899a131241639d620b5fb7c02a586b/MeasurementDataset/README.md -------------------------------------------------------------------------------- /MeasurementDataset/SineReferenceGeneration.m: -------------------------------------------------------------------------------- 1 | BaseFrequency = 0.1; 2 | freqRate = 0.2; 3 | 4 | ReferenceGenerationStep = (0:4000)'; 5 | SampleRate = 200; 6 | dt = 1/SampleRate; 7 | 8 | % f = a * sin(2*pi*freq(t)*t) = a * sin(g(t)) 9 | % g = 2*pi*freq(t)*t 10 | % freq(t) = BaseFrequency + freqRate * t 11 | % dfdt = a * (dfdg * dgdt) = a * cos(g(t)) * dgdt 12 | % dgdt = 2*pi* (dfreqdt*t + freq(t)) = 2*pi* (freqRate*t + freq(t)) 13 | 14 | t = ReferenceGenerationStep/SampleRate; 15 | freq = (BaseFrequency + freqRate*t); 16 | g = 2*pi*freq.*t; 17 | a = deg2rad(3); 18 | f = a * sin(g); 19 | dgdt = 2*pi* (freqRate*t + freq); 20 | dfdt = a * cos(g) .* dgdt; 21 | 22 | 23 | dfdt_numerical = diff(f) ./ diff(t); 24 | t_dfdt = t(1:(end-1)) + diff(t)/2; 25 | 26 | figure(1); 27 | subplot(2,1,1); 28 | plot(t, rad2deg(f)); 29 | subplot(2,1,2); 30 | plot(t_dfdt, dfdt_numerical, t, dfdt); 31 | -------------------------------------------------------------------------------- /Misc/ExtractHeading.m: -------------------------------------------------------------------------------- 1 | function heading = ExtractHeading(q) 2 | Phi = @(q)[q(1) -q(2) -q(3) -q(4); % for q o p = Phi(q) * p 3 | q(2) q(1) -q(4) q(3); 4 | q(3) q(4) q(1) -q(2); 5 | q(4) -q(3) q(2) q(1)]; 6 | Gamma = @(p)[p(1) -p(2) -p(3) -p(4); % for q o p = Gamma(p) * q 7 | p(2) p(1) p(4) -p(3); 8 | p(3) -p(4) p(1) p(2); 9 | p(4) p(3) -p(2) p(1)]; 10 | 11 | devec = [0,1,0,0;0,0,1,0;0,0,0,1]; % 'v' in notes 12 | vec = [0,0,0;1,0,0;0,1,0;0,0,1]; % '^' in notes 13 | 14 | %% Derive tilt and heading from combined quaternion 15 | % X unit vector of Body in Inertial frame 16 | I_e_X = devec * Phi(q) * Gamma(q)' * [0;1;0;0]; 17 | 18 | % Extract direction of the X-vector 19 | direction = I_e_X(1:2); %direction = [eye(2), zeros(2,1)] * I_e_X; 20 | % This direction vector points in the heading direction 21 | heading = atan2(direction(2), direction(1)); -------------------------------------------------------------------------------- /Misc/HeadingQuaternion.m: -------------------------------------------------------------------------------- 1 | function q_heading = HeadingQuaternion(q) 2 | Phi = @(q)[q(1) -q(2) -q(3) -q(4); % for q o p = Phi(q) * p 3 | q(2) q(1) -q(4) q(3); 4 | q(3) q(4) q(1) -q(2); 5 | q(4) -q(3) q(2) q(1)]; 6 | Gamma = @(p)[p(1) -p(2) -p(3) -p(4); % for q o p = Gamma(p) * q 7 | p(2) p(1) p(4) -p(3); 8 | p(3) -p(4) p(1) p(2); 9 | p(4) p(3) -p(2) p(1)]; 10 | 11 | devec = [0,1,0,0;0,0,1,0;0,0,0,1]; % 'v' in notes 12 | vec = [0,0,0;1,0,0;0,1,0;0,0,1]; % '^' in notes 13 | 14 | %% Derive tilt and heading from combined quaternion 15 | % X unit vector of Body in Inertial frame 16 | I_e_X = devec * Phi(q) * Gamma(q)' * [0;1;0;0]; 17 | 18 | % Extract direction of the X-vector 19 | direction = I_e_X(1:2); %direction = [eye(2), zeros(2,1)] * I_e_X; 20 | % This direction vector points in the heading direction 21 | heading = atan2(direction(2), direction(1)); 22 | 23 | % Construct heading quaternion 24 | q_heading = [cos(heading/2); 25 | 0; 26 | 0; 27 | sin(heading/2)]; 28 | 29 | if (q(1) < 0) % scalar is negative, make sure this is the case in the output quaternion too 30 | q_heading = -q_heading; 31 | end -------------------------------------------------------------------------------- /Misc/MEX tests/README.md: -------------------------------------------------------------------------------- 1 | These are just some illustrational examples of how to write MEX files and are not used anywhere. -------------------------------------------------------------------------------- /Misc/MEX tests/persistant_test.c: -------------------------------------------------------------------------------- 1 | // test.c 2 | #include 3 | #include "mex.h" 4 | 5 | static double *myarray = NULL; 6 | double *pr; 7 | 8 | void exitFcn() { 9 | if (myarray != NULL) 10 | mxFree(myarray); 11 | } 12 | 13 | void 14 | mexFunction( int nlhs, // number of output arguments 15 | mxArray *plhs[], // output data containers 16 | int nrhs, // number of input arguments 17 | const mxArray *prhs[] // input data 18 | ) 19 | { 20 | if (nrhs < 1) 21 | mexErrMsgTxt("Must have one non-string input"); 22 | if (mxIsChar(prhs[0])) 23 | mexErrMsgTxt("Must have one non-string input"); 24 | 25 | if (myarray==NULL) { 26 | /* since myarray is initialized to NULL, we know 27 | this is the first call of the MEX-function 28 | after it was loaded. Therefore, we should 29 | set up myarray and the exit function. */ 30 | /* Allocate array. Use mexMackMemoryPersistent to make the allocated memory persistent in subsequent calls*/ 31 | mexPrintf("First call to MEX-file\n"); 32 | myarray = mxCalloc(1,8); 33 | mexMakeMemoryPersistent(myarray); 34 | mexAtExit(exitFcn); 35 | } 36 | mexPrintf("Old string was '%f'.\n",myarray[0]); 37 | pr = mxGetPr(prhs[0]); 38 | mexPrintf("New string is '%f'.\n",pr[0]); 39 | memcpy((char*)myarray,(char*)mxGetPr(prhs[0]), sizeof(double)); 40 | } -------------------------------------------------------------------------------- /Misc/Quaternion2ZYXEulerAngles.m: -------------------------------------------------------------------------------- 1 | function rpy = QuaternionToZYXEulerAngles(q) 2 | % Normalize if incorrect (due to small numerical errors) 3 | q = q / norm(q); 4 | 5 | % Extract quaternion elements 6 | qw = q(1); 7 | qx = q(2); 8 | qy = q(3); 9 | qz = q(4); 10 | 11 | % Ensure asin stays within -1 to 1 since asin of values larger than 1 produces complex number 12 | aSinInput = -2*(qx*qz-qw*qy); 13 | aSinInput(aSinInput > 1) = 1; 14 | aSinInput(aSinInput < -1) = -1; 15 | 16 | % Compute Euler ZYX angles 17 | roll = atan2( 2*(qy*qz+qw*qx), qw^2 - qx^2 - qy^2 + qz^2 ); 18 | pitch = asin( aSinInput ); 19 | yaw = atan2( 2*(qx*qy+qw*qz), qw^2 + qx^2 - qy^2 - qz^2 ); 20 | 21 | rpy = [roll; pitch; yaw]; -------------------------------------------------------------------------------- /Misc/QuaternionError.m: -------------------------------------------------------------------------------- 1 | function q_err = QuaternionError(q, q_ref) 2 | Phi = @(q)[q(1) -q(2) -q(3) -q(4); % for q o p = Phi(q) * p 3 | q(2) q(1) -q(4) q(3); 4 | q(3) q(4) q(1) -q(2); 5 | q(4) -q(3) q(2) q(1)]; 6 | Gamma = @(p)[p(1) -p(2) -p(3) -p(4); % for q o p = Gamma(p) * q 7 | p(2) p(1) p(4) -p(3); 8 | p(3) -p(4) p(1) p(2); 9 | p(4) p(3) -p(2) p(1)]; 10 | 11 | devec = [0,1,0,0;0,0,1,0;0,0,0,1]; % 'v' in notes 12 | vec = [0,0,0;1,0,0;0,1,0;0,0,1]; % '^' in notes 13 | 14 | % Compute quaternion error based on reference input 15 | q_err = Phi(q_ref)' * q; % quaternion error in body frame 16 | 17 | if (q_err(1) < 0) 18 | q_err = -q_err; % inverse quaternion gives the shorter way to the same rotation 19 | end 20 | end -------------------------------------------------------------------------------- /Misc/ReplaceStringInFile.m: -------------------------------------------------------------------------------- 1 | function ReplaceStringInFile(filename, strToFind, strToReplaceWith) 2 | tmpfilename = ['tmp_' filename]; 3 | fidi=fopen(filename,'r'); 4 | fido=fopen(tmpfilename,'w'); 5 | while ~feof(fidi) 6 | l=fgetl(fidi); % read line 7 | l = strrep(l, strToFind, strToReplaceWith); 8 | fprintf(fido,'%s\n',l); % 'fgetl returns \n so it's embedded 9 | end 10 | fidi=fclose(fidi); 11 | fido=fclose(fido); 12 | delete(filename); 13 | movefile(tmpfilename, filename); 14 | end -------------------------------------------------------------------------------- /Misc/ZYXEulerAngles2Quaternion.m: -------------------------------------------------------------------------------- 1 | function q = ZYXEulerAngles2Quaternion(rpy) 2 | c = cos(rpy/2); 3 | s = sin(rpy/2); 4 | 5 | % Compute quaternion elements 6 | qw = c(3)*c(2)*c(1)+s(3)*s(2)*s(1); 7 | qx = c(3)*c(2)*s(1)-s(3)*s(2)*c(1); 8 | qy = c(3)*s(2)*c(1)+s(3)*c(2)*s(1); 9 | qz = s(3)*c(2)*c(1)-c(3)*s(2)*s(1); 10 | 11 | % Assemble quaternion 12 | q = [qw; qx; qy; qz]; 13 | end -------------------------------------------------------------------------------- /Misc/omega_b2qdot.m: -------------------------------------------------------------------------------- 1 | function dq = omega_b2qdot(q, omega_b) 2 | Phi = @(q)[q(1) -q(2) -q(3) -q(4); % for q o p = Phi(q) * p 3 | q(2) q(1) -q(4) q(3); 4 | q(3) q(4) q(1) -q(2); 5 | q(4) -q(3) q(2) q(1)]; 6 | Gamma = @(p)[p(1) -p(2) -p(3) -p(4); % for q o p = Gamma(p) * q 7 | p(2) p(1) p(4) -p(3); 8 | p(3) -p(4) p(1) p(2); 9 | p(4) p(3) -p(2) p(1)]; 10 | 11 | devec = [0,1,0,0;0,0,1,0;0,0,0,1]; % 'v' in notes 12 | vec = [0,0,0;1,0,0;0,1,0;0,0,1]; % '^' in notes 13 | 14 | dq = 1/2 * Phi(q) * vec * omega_b; 15 | end -------------------------------------------------------------------------------- /Misc/qdot2omega_b.m: -------------------------------------------------------------------------------- 1 | function omega_b = qdot2omega_b(q, dq) 2 | Phi = @(q)[q(1) -q(2) -q(3) -q(4); % for q o p = Phi(q) * p 3 | q(2) q(1) -q(4) q(3); 4 | q(3) q(4) q(1) -q(2); 5 | q(4) -q(3) q(2) q(1)]; 6 | Gamma = @(p)[p(1) -p(2) -p(3) -p(4); % for q o p = Gamma(p) * q 7 | p(2) p(1) p(4) -p(3); 8 | p(3) -p(4) p(1) p(2); 9 | p(4) p(3) -p(2) p(1)]; 10 | 11 | devec = [0,1,0,0;0,0,1,0;0,0,0,1]; % 'v' in notes 12 | vec = [0,0,0;1,0,0;0,1,0;0,0,1]; % '^' in notes 13 | 14 | omega_b = 2 * devec * Phi(q)' * dq; 15 | end -------------------------------------------------------------------------------- /Model/EvaluateCompleteODE.m: -------------------------------------------------------------------------------- 1 | % Evaluate the non-linear ODE by using the generated Euler-Lagrange 2 | % matrices. To run the function remember to add the "generated" folder to 3 | % the MATLAB path: addpath('generated'); 4 | % dX returns the derivative of the complete state vector: 5 | % X = [x; y; q; dx; dy; dq] 6 | function dX = EvaluateCompleteODE(X, tau, COM_X,COM_Y,COM_Z,Jbx,Jby,Jbz,Jk,Jw,Mb,Mk,Bvb,Bvk,Bvm,rk,rw,g) 7 | x = X(1); 8 | y = X(2); 9 | q = X(3:6); q1 = X(3); q2 = X(4); q3 = X(5); q4 = X(6); 10 | dx = X(7); 11 | dy = X(8); 12 | dq = X(9:12); dq1 = X(9); dq2 = X(10); dq3 = X(11); dq4 = X(12); 13 | 14 | chi = X(1:6); 15 | dchi = X(7:12); 16 | 17 | %% Calculate non-linear dynamics given constants (and without regularization, hence beta=0) 18 | beta = 0; 19 | M_tilde = mass(COM_X,COM_Y,COM_Z,Jbx,Jby,Jbz,Jk,Jw,Mb,Mk,q1,q2,q3,q4,rk,rw); 20 | C_tilde = coriolis(COM_X,COM_Y,COM_Z,Jbx,Jby,Jbz,Jw,Mb,beta,dq1,dq2,dq3,dq4,dx,dy,q1,q2,q3,q4,rk,rw); 21 | G_tilde = gravity(COM_X,COM_Y,COM_Z,Mb,beta,g,q1,q2,q3,q4); 22 | D_tilde = friction(Bvb,Bvk,Bvm,beta,dq1,dq2,dq3,dq4,dx,dy,q1,q2,q3,q4,rk,rw); 23 | Q_tilde = input_forces(q1,q2,q3,q4,rk,rw); 24 | 25 | %% Compute Non-linear ODE 26 | f = M_tilde \ ( - C_tilde*dchi - G_tilde - D_tilde ); 27 | g = M_tilde \ Q_tilde; 28 | ddchi = f + g*tau; 29 | 30 | dX = [dchi; ddchi]; -------------------------------------------------------------------------------- /Model/functions/SimplifyWithQuatConstraint.m: -------------------------------------------------------------------------------- 1 | function simplifiedFun = SimplifyWithQuatConstraint(fun, q) 2 | simplifiedFun = subs(simplify(subs(fun, q(1)^2+q(2)^2+q(3)^2+q(4)^2, 1)), q(1)^2+q(2)^2+q(3)^2+q(4)^2, 1); -------------------------------------------------------------------------------- /Model/generated/ForwardKinematics.m: -------------------------------------------------------------------------------- 1 | function dxdy = ForwardKinematics(dpsi1,dpsi2,dpsi3,dq1,dq2,dq3,dq4,q1,q2,q3,q4,rk,rw) 2 | %FORWARDKINEMATICS 3 | % DXDY = FORWARDKINEMATICS(DPSI1,DPSI2,DPSI3,DQ1,DQ2,DQ3,DQ4,Q1,Q2,Q3,Q4,RK,RW) 4 | 5 | % This function was generated by the Symbolic Math Toolbox version 8.1. 6 | % 13-Feb-2019 11:02:59 7 | 8 | t2 = sqrt(2.0); 9 | t3 = 1.0./rk; 10 | t4 = dpsi1.*-2.0+dpsi2+dpsi3; 11 | t5 = q1.^2; 12 | t6 = q2.^2; 13 | t7 = q3.^2; 14 | t8 = q4.^2; 15 | t9 = sqrt(6.0); 16 | t10 = dpsi2-dpsi3; 17 | t11 = q1.*q4.*2.0; 18 | t12 = q2.*q3.*2.0; 19 | t13 = dpsi1+dpsi2+dpsi3; 20 | dxdy = [-rk.*(dq1.*q3.*2.0-dq3.*q1.*2.0-dq2.*q4.*2.0+dq4.*q2.*2.0+rw.*t2.*t3.*t13.*(q1.*q2.*2.0-q3.*q4.*2.0).*(1.0./3.0)+rw.*t3.*t9.*t10.*(t5-t6+t7-t8).*(1.0./3.0)-rw.*t2.*t3.*t4.*(t11+t12).*(1.0./3.0));-rk.*(dq1.*q2.*-2.0+dq2.*q1.*2.0-dq3.*q4.*2.0+dq4.*q3.*2.0+rw.*t2.*t3.*t13.*(q1.*q3.*2.0+q2.*q4.*2.0).*(1.0./3.0)+rw.*t2.*t3.*t4.*(t5+t6-t7-t8).*(1.0./3.0)+rw.*t3.*t9.*t10.*(t11-t12).*(1.0./3.0))]; 21 | -------------------------------------------------------------------------------- /Model/generated/ForwardKinematicsInHeadingFrame.m: -------------------------------------------------------------------------------- 1 | function dxdy_h = ForwardKinematicsInHeadingFrame(dpsi1,dpsi2,dpsi3,dq1,dq2,dq3,dq4,q1,q2,q3,q4,rk,rw) 2 | %FORWARDKINEMATICSINHEADINGFRAME 3 | % DXDY_H = FORWARDKINEMATICSINHEADINGFRAME(DPSI1,DPSI2,DPSI3,DQ1,DQ2,DQ3,DQ4,Q1,Q2,Q3,Q4,RK,RW) 4 | 5 | % This function was generated by the Symbolic Math Toolbox version 8.1. 6 | % 13-Feb-2019 11:03:00 7 | 8 | t2 = sqrt(6.0); 9 | t3 = q1.^2; 10 | t4 = q2.^2; 11 | t5 = q3.^2; 12 | t6 = q4.^2; 13 | t7 = sqrt(2.0); 14 | dxdy_h = [dq1.*q3.*rk.*-2.0+dq3.*q1.*rk.*2.0-dq2.*q4.*rk.*2.0+dq4.*q2.*rk.*2.0-dpsi2.*rw.*t2.*t3.*(1.0./3.0)-dpsi2.*rw.*t2.*t4.*(1.0./3.0)+dpsi3.*rw.*t2.*t3.*(1.0./3.0)-dpsi2.*rw.*t2.*t5.*(1.0./3.0)+dpsi3.*rw.*t2.*t4.*(1.0./3.0)-dpsi2.*rw.*t2.*t6.*(1.0./3.0)+dpsi3.*rw.*t2.*t5.*(1.0./3.0)+dpsi3.*rw.*t2.*t6.*(1.0./3.0);dq1.*q2.*rk.*2.0-dq2.*q1.*rk.*2.0-dq3.*q4.*rk.*2.0+dq4.*q3.*rk.*2.0+dpsi1.*rw.*t3.*t7.*(2.0./3.0)+dpsi1.*rw.*t4.*t7.*(2.0./3.0)-dpsi2.*rw.*t3.*t7.*(1.0./3.0)+dpsi1.*rw.*t5.*t7.*(2.0./3.0)-dpsi2.*rw.*t4.*t7.*(1.0./3.0)-dpsi3.*rw.*t3.*t7.*(1.0./3.0)+dpsi1.*rw.*t6.*t7.*(2.0./3.0)-dpsi2.*rw.*t5.*t7.*(1.0./3.0)-dpsi3.*rw.*t4.*t7.*(1.0./3.0)-dpsi2.*rw.*t6.*t7.*(1.0./3.0)-dpsi3.*rw.*t5.*t7.*(1.0./3.0)-dpsi3.*rw.*t6.*t7.*(1.0./3.0)]; 15 | -------------------------------------------------------------------------------- /Model/generated/ForwardKinematics_2L.m: -------------------------------------------------------------------------------- 1 | function dxdy_2l = ForwardKinematics_2L(dpsi1,dpsi2,dpsi3,dq1,dq2,dq3,dq4,l,q1,q2,q3,q4,rk,rw) 2 | %FORWARDKINEMATICS_2L 3 | % DXDY_2L = FORWARDKINEMATICS_2L(DPSI1,DPSI2,DPSI3,DQ1,DQ2,DQ3,DQ4,L,Q1,Q2,Q3,Q4,RK,RW) 4 | 5 | % This function was generated by the Symbolic Math Toolbox version 8.1. 6 | % 09-Nov-2018 14:51:25 7 | 8 | t2 = sqrt(2.0); 9 | t3 = 1.0./rk; 10 | t4 = dq1.*q3.*2.0; 11 | t5 = dq4.*q2.*2.0; 12 | t6 = dpsi1.*-2.0+dpsi2+dpsi3; 13 | t7 = q1.^2; 14 | t8 = q2.^2; 15 | t9 = q3.^2; 16 | t10 = q4.^2; 17 | t11 = sqrt(6.0); 18 | t12 = dpsi2-dpsi3; 19 | t13 = q1.*q4.*2.0; 20 | t14 = q2.*q3.*2.0; 21 | t15 = dpsi1+dpsi2+dpsi3; 22 | t16 = dq2.*q1.*2.0; 23 | t17 = dq4.*q3.*2.0; 24 | dxdy_2l = [-rk.*(t4+t5-dq3.*q1.*2.0-dq2.*q4.*2.0+rw.*t2.*t3.*t15.*(q1.*q2.*2.0-q3.*q4.*2.0).*(1.0./3.0)+rw.*t3.*t11.*t12.*(t7-t8+t9-t10).*(1.0./3.0)-rw.*t2.*t3.*t6.*(t13+t14).*(1.0./3.0))+l.*(t4+t5+dq3.*q1.*2.0+dq2.*q4.*2.0).*2.0;-rk.*(t16+t17-dq1.*q2.*2.0-dq3.*q4.*2.0+rw.*t2.*t3.*t15.*(q1.*q3.*2.0+q2.*q4.*2.0).*(1.0./3.0)+rw.*t2.*t3.*t6.*(t7+t8-t9-t10).*(1.0./3.0)+rw.*t3.*t11.*t12.*(t13-t14).*(1.0./3.0))-l.*(t16-t17+dq1.*q2.*2.0-dq3.*q4.*2.0).*2.0]; 25 | -------------------------------------------------------------------------------- /Model/generated/ForwardKinematics_COM.m: -------------------------------------------------------------------------------- 1 | function dxdy_com = ForwardKinematics_COM(dpsi1,dpsi2,dpsi3,dq1,dq2,dq3,dq4,l,q1,q2,q3,q4,rk,rw) 2 | %FORWARDKINEMATICS_COM 3 | % DXDY_COM = FORWARDKINEMATICS_COM(DPSI1,DPSI2,DPSI3,DQ1,DQ2,DQ3,DQ4,L,Q1,Q2,Q3,Q4,RK,RW) 4 | 5 | % This function was generated by the Symbolic Math Toolbox version 8.1. 6 | % 09-Nov-2018 14:51:24 7 | 8 | t2 = sqrt(2.0); 9 | t3 = 1.0./rk; 10 | t4 = dq1.*q3.*2.0; 11 | t5 = dq4.*q2.*2.0; 12 | t6 = dpsi1.*-2.0+dpsi2+dpsi3; 13 | t7 = q1.^2; 14 | t8 = q2.^2; 15 | t9 = q3.^2; 16 | t10 = q4.^2; 17 | t11 = sqrt(6.0); 18 | t12 = dpsi2-dpsi3; 19 | t13 = q1.*q4.*2.0; 20 | t14 = q2.*q3.*2.0; 21 | t15 = dpsi1+dpsi2+dpsi3; 22 | t16 = dq2.*q1.*2.0; 23 | t17 = dq4.*q3.*2.0; 24 | dxdy_com = [-rk.*(t4+t5-dq3.*q1.*2.0-dq2.*q4.*2.0+rw.*t2.*t3.*t15.*(q1.*q2.*2.0-q3.*q4.*2.0).*(1.0./3.0)+rw.*t3.*t11.*t12.*(t7-t8+t9-t10).*(1.0./3.0)-rw.*t2.*t3.*t6.*(t13+t14).*(1.0./3.0))+l.*(t4+t5+dq3.*q1.*2.0+dq2.*q4.*2.0);-rk.*(t16+t17-dq1.*q2.*2.0-dq3.*q4.*2.0+rw.*t2.*t3.*t15.*(q1.*q3.*2.0+q2.*q4.*2.0).*(1.0./3.0)+rw.*t2.*t3.*t6.*(t7+t8-t9-t10).*(1.0./3.0)+rw.*t3.*t11.*t12.*(t13-t14).*(1.0./3.0))-l.*(t16-t17+dq1.*q2.*2.0-dq3.*q4.*2.0)]; 25 | -------------------------------------------------------------------------------- /Model/generated/ForwardKinematics_WithRot.m: -------------------------------------------------------------------------------- 1 | function dxdydyaw = ForwardKinematics_WithRot(dpsi1,dpsi2,dpsi3,dq1,dq2,dq3,dq4,q1,q2,q3,q4,rk,rw) 2 | %FORWARDKINEMATICS_WITHROT 3 | % DXDYDYAW = FORWARDKINEMATICS_WITHROT(DPSI1,DPSI2,DPSI3,DQ1,DQ2,DQ3,DQ4,Q1,Q2,Q3,Q4,RK,RW) 4 | 5 | % This function was generated by the Symbolic Math Toolbox version 8.1. 6 | % 13-Feb-2019 11:03:00 7 | 8 | t2 = sqrt(2.0); 9 | t3 = 1.0./rk; 10 | t4 = dpsi1.*-2.0+dpsi2+dpsi3; 11 | t5 = q1.^2; 12 | t6 = q2.^2; 13 | t7 = q3.^2; 14 | t8 = q4.^2; 15 | t9 = sqrt(6.0); 16 | t10 = dpsi2-dpsi3; 17 | t11 = q1.*q4.*2.0; 18 | t12 = q2.*q3.*2.0; 19 | t13 = dpsi1+dpsi2+dpsi3; 20 | dxdydyaw = [-rk.*(dq1.*q3.*2.0-dq3.*q1.*2.0-dq2.*q4.*2.0+dq4.*q2.*2.0+rw.*t2.*t3.*t13.*(q1.*q2.*2.0-q3.*q4.*2.0).*(1.0./3.0)+rw.*t3.*t9.*t10.*(t5-t6+t7-t8).*(1.0./3.0)-rw.*t2.*t3.*t4.*(t11+t12).*(1.0./3.0));-rk.*(dq1.*q2.*-2.0+dq2.*q1.*2.0-dq3.*q4.*2.0+dq4.*q3.*2.0+rw.*t2.*t3.*t13.*(q1.*q3.*2.0+q2.*q4.*2.0).*(1.0./3.0)+rw.*t2.*t3.*t4.*(t5+t6-t7-t8).*(1.0./3.0)+rw.*t3.*t9.*t10.*(t11-t12).*(1.0./3.0));dpsi1.*rw.*t2.*t3.*(1.0./3.0)+dpsi2.*rw.*t2.*t3.*(1.0./3.0)+dpsi3.*rw.*t2.*t3.*(1.0./3.0)]; 21 | -------------------------------------------------------------------------------- /Model/generated/PlanarInverseKinematics.m: -------------------------------------------------------------------------------- 1 | function dpsi = PlanarInverseKinematics(dphi,dtheta,rk,rw) 2 | %PLANARINVERSEKINEMATICS 3 | % DPSI = PLANARINVERSEKINEMATICS(DPHI,DTHETA,RK,RW) 4 | 5 | % This function was generated by the Symbolic Math Toolbox version 8.1. 6 | % 02-Apr-2019 09:02:09 7 | 8 | dpsi = (rk.*(dphi-dtheta))./rw; 9 | -------------------------------------------------------------------------------- /Model/generated/SymbolicModel.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mindThomas/Kugle-MATLAB/b1ee16de78899a131241639d620b5fb7c02a586b/Model/generated/SymbolicModel.mat -------------------------------------------------------------------------------- /Model/generated/gravity.m: -------------------------------------------------------------------------------- 1 | function G = gravity(COM_X,COM_Y,COM_Z,Mb,beta,g,q1,q2,q3,q4) 2 | %GRAVITY 3 | % G = GRAVITY(COM_X,COM_Y,COM_Z,MB,BETA,G,Q1,Q2,Q3,Q4) 4 | 5 | % This function was generated by the Symbolic Math Toolbox version 8.1. 6 | % 12-Nov-2018 22:44:45 7 | 8 | t2 = COM_X.*q1; 9 | t3 = COM_Z.*q3; 10 | t15 = COM_Y.*q4; 11 | t4 = t2+t3-t15; 12 | t5 = COM_Y.*q2; 13 | t6 = COM_Z.*q1; 14 | t17 = COM_X.*q3; 15 | t7 = t5+t6-t17; 16 | t8 = COM_X.*q2; 17 | t9 = COM_Y.*q3; 18 | t10 = COM_Z.*q4; 19 | t11 = t8+t9+t10; 20 | t12 = COM_Y.*q1; 21 | t13 = COM_X.*q4; 22 | t16 = COM_Z.*q2; 23 | t14 = t12+t13-t16; 24 | G = [0.0;0.0;Mb.*g.*q4.*t4.*-2.0-Mb.*g.*q2.*t7.*2.0-Mb.*g.*q3.*t11.*2.0+Mb.*g.*q1.*t14.*2.0;Mb.*g.*q1.*t4.*-2.0-Mb.*g.*q3.*t7.*2.0+Mb.*g.*q2.*t11.*2.0-Mb.*g.*q4.*t14.*2.0;Mb.*g.*q2.*t4.*2.0-Mb.*g.*q4.*t7.*2.0+Mb.*g.*q1.*t11.*2.0+Mb.*g.*q3.*t14.*2.0;beta.*q1.^2.*(1.0./2.0)+beta.*q2.^2.*(1.0./2.0)+beta.*q3.^2.*(1.0./2.0)+beta.*q4.^2.*(1.0./2.0)]; 25 | -------------------------------------------------------------------------------- /Model/generated/planar_B_matrix.m: -------------------------------------------------------------------------------- 1 | function B = planar_B_matrix(Jb,Jk,Jw,Mb,Mk,l,rk,rw,theta) 2 | %PLANAR_B_MATRIX 3 | % B = PLANAR_B_MATRIX(JB,JK,JW,MB,MK,L,RK,RW,THETA) 4 | 5 | % This function was generated by the Symbolic Math Toolbox version 8.1. 6 | % 02-Apr-2019 09:02:21 7 | 8 | t2 = rk.^2; 9 | t3 = t2.^2; 10 | t4 = l.^2; 11 | t5 = rw.^2; 12 | t6 = cos(theta); 13 | t7 = Mb.^2; 14 | t8 = Mb.*l.*rk.*t6; 15 | t9 = Jb.*Jk.*t5; 16 | t10 = Jb.*Jw.*t2; 17 | t11 = Jk.*Jw.*t2; 18 | t12 = Jw.*Mb.*t3; 19 | t13 = Jw.*Mk.*t3; 20 | t14 = t2.*t4.*t5.*t7; 21 | t15 = Jk.*Mb.*t4.*t5; 22 | t16 = Jw.*Mb.*t2.*t4; 23 | t17 = Jb.*Mb.*t2.*t5; 24 | t18 = Jb.*Mk.*t2.*t5; 25 | t19 = Jw.*Mb.*l.*rk.*t2.*t6.*2.0; 26 | t20 = Mb.*Mk.*t2.*t4.*t5; 27 | t21 = t6.^2; 28 | t22 = t9+t10+t11+t12+t13+t14+t15+t16+t17+t18+t19+t20-t2.*t4.*t5.*t7.*t21; 29 | t23 = 1.0./t22; 30 | B = [0.0;0.0;rk.*rw.*t23.*(Jb+t8+Mb.*t4);-rk.*rw.*t23.*(Jk+t8+Mb.*t2+Mk.*t2)]; 31 | -------------------------------------------------------------------------------- /Model/generated/planar_coriolis.m: -------------------------------------------------------------------------------- 1 | function C = planar_coriolis(Mb,dtheta,l,rk,theta) 2 | %PLANAR_CORIOLIS 3 | % C = PLANAR_CORIOLIS(MB,DTHETA,L,RK,THETA) 4 | 5 | % This function was generated by the Symbolic Math Toolbox version 8.1. 6 | % 02-Apr-2019 09:02:11 7 | 8 | C = reshape([0.0,0.0,-Mb.*dtheta.*l.*rk.*sin(theta),0.0],[2,2]); 9 | -------------------------------------------------------------------------------- /Model/generated/planar_friction.m: -------------------------------------------------------------------------------- 1 | function D = planar_friction(Bvb,Bvk,Bvm,dphi,dtheta,rk,rw) 2 | %PLANAR_FRICTION 3 | % D = PLANAR_FRICTION(BVB,BVK,BVM,DPHI,DTHETA,RK,RW) 4 | 5 | % This function was generated by the Symbolic Math Toolbox version 8.1. 6 | % 02-Apr-2019 09:02:11 7 | 8 | t2 = rk.^2; 9 | t3 = 1.0./rw.^2; 10 | t4 = dphi-dtheta; 11 | t5 = Bvm.*t2.*t3.*t4; 12 | D = [t5+Bvk.*dphi.*rk;-t5+Bvb.*dtheta]; 13 | -------------------------------------------------------------------------------- /Model/generated/planar_gravity.m: -------------------------------------------------------------------------------- 1 | function G = planar_gravity(Mb,g,l,theta) 2 | %PLANAR_GRAVITY 3 | % G = PLANAR_GRAVITY(MB,G,L,THETA) 4 | 5 | % This function was generated by the Symbolic Math Toolbox version 8.1. 6 | % 02-Apr-2019 09:02:11 7 | 8 | G = [0.0;-Mb.*g.*l.*sin(theta)]; 9 | -------------------------------------------------------------------------------- /Model/generated/planar_input_forces.m: -------------------------------------------------------------------------------- 1 | function Q = planar_input_forces(rk,rw,tau) 2 | %PLANAR_INPUT_FORCES 3 | % Q = PLANAR_INPUT_FORCES(RK,RW,TAU) 4 | 5 | % This function was generated by the Symbolic Math Toolbox version 8.1. 6 | % 02-Apr-2019 09:02:11 7 | 8 | t2 = 1.0./rw; 9 | t3 = rk.*t2.*tau; 10 | Q = [t3;-t3]; 11 | -------------------------------------------------------------------------------- /Model/generated/planar_mass.m: -------------------------------------------------------------------------------- 1 | function M = planar_mass(Jb,Jk,Jw,Mb,Mk,l,rk,rw,theta) 2 | %PLANAR_MASS 3 | % M = PLANAR_MASS(JB,JK,JW,MB,MK,L,RK,RW,THETA) 4 | 5 | % This function was generated by the Symbolic Math Toolbox version 8.1. 6 | % 02-Apr-2019 09:02:10 7 | 8 | t2 = rk.^2; 9 | t3 = 1.0./rw.^2; 10 | t4 = Jw.*t2.*t3; 11 | t5 = cos(theta); 12 | t6 = Mb.*l.*rk.*t5; 13 | t7 = -t4+t6; 14 | M = reshape([Jk+t4+Mb.*t2+Mk.*t2,t7,t7,Jb+t4+Mb.*l.^2],[2,2]); 15 | -------------------------------------------------------------------------------- /Parameters/Constants_nominal.m: -------------------------------------------------------------------------------- 1 | % Let the nominal parameters be the same as the model parameters except for 2 | % the parameters changed below 3 | 4 | % No friction in nominal model 5 | Bvk_nominal = 0; 6 | Bvm_nominal = 0; 7 | Bvb_nominal = 0; 8 | 9 | Mb_nominal = Mb; 10 | Jbx_nominal = Jbx; 11 | Jby_nominal = Jby; 12 | Jbz_nominal = Jbz; 13 | 14 | % Center of mass is assumed to be right above center of ball 15 | %COM_nominal = [0, 0, l]'; 16 | COM_nominal = COM; 17 | 18 | %% Assemble constant vector used in simulation 19 | constants_nominal = [Jk, Mk, rk, Mb_nominal, Jbx_nominal, Jby_nominal, Jbz_nominal, Jw, rw, Bvk_nominal, Bvm_nominal, Bvb_nominal, g]'; -------------------------------------------------------------------------------- /Parameters/Parameters_Controllers.m: -------------------------------------------------------------------------------- 1 | % General sample rate 2 | SampleRate = 200; % 200 Hz controller sample rate 3 | Ts = 1/SampleRate; 4 | 5 | % Torque output parameter 6 | SaturationTorqueOfMaxOutputTorque = 0.8; % saturation percentage of maximum output torque 7 | 8 | %% Sliding Mode controller selection and gains 9 | SlidingManifold = 1; % 0=q_dot inertial, 1=q_dot body, 2=Body angular velocity, 3=Inertial angular velocity 10 | SwitchingLaw = 2; % 1=discontinous, 2=continous 11 | 12 | AggressiveGains = true; % true = aggressive gains, false = non-aggressive gains 13 | if (AggressiveGains) 14 | K = diag([15, 15, 6]); 15 | eta = [6; 6; 3]; 16 | epsilon = [0.5; 0.5; 0.2]; 17 | else 18 | K = diag([6, 6, 6]); 19 | eta = [5; 5; 6]; 20 | epsilon = [0.8; 0.8; 0.3]; 21 | end 22 | 23 | %% Velocity controller 24 | VelocityLQR_IntegralEnabled = false; 25 | VelocityLQR_PositionControlAtZeroVelocityReference = true; 26 | VelocityLQR_MaximumKickinVelocity = 0.1; % m/s 27 | VelocityLQR_IntegratorPowerupStabilizeTime = 3.0; % wait 3 seconds in the beginning for integrator to settle (and before allowing manual movement) 28 | VelocityController_StabilizationDetectionVelocity = 0.2; % if the robot is pushed with a velocity of more than 0.2 m/s after the initialization time the initialization integrator will be disabled allowing manual movement -------------------------------------------------------------------------------- /Parameters/Parameters_General.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mindThomas/Kugle-MATLAB/b1ee16de78899a131241639d620b5fb7c02a586b/Parameters/Parameters_General.m -------------------------------------------------------------------------------- /Simulation/KugleSim_LQR.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mindThomas/Kugle-MATLAB/b1ee16de78899a131241639d620b5fb7c02a586b/Simulation/KugleSim_LQR.slx -------------------------------------------------------------------------------- /Simulation/KugleSim_MPC.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mindThomas/Kugle-MATLAB/b1ee16de78899a131241639d620b5fb7c02a586b/Simulation/KugleSim_MPC.slx -------------------------------------------------------------------------------- /Simulation/KugleSim_SlidingMode.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mindThomas/Kugle-MATLAB/b1ee16de78899a131241639d620b5fb7c02a586b/Simulation/KugleSim_SlidingMode.slx -------------------------------------------------------------------------------- /Simulation/KugleSim_VelocityLQR.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mindThomas/Kugle-MATLAB/b1ee16de78899a131241639d620b5fb7c02a586b/Simulation/KugleSim_VelocityLQR.slx -------------------------------------------------------------------------------- /Simulation/MPC_Simulation.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mindThomas/Kugle-MATLAB/b1ee16de78899a131241639d620b5fb7c02a586b/Simulation/MPC_Simulation.slx -------------------------------------------------------------------------------- /Simulation/subsystems/Balance_LQR.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mindThomas/Kugle-MATLAB/b1ee16de78899a131241639d620b5fb7c02a586b/Simulation/subsystems/Balance_LQR.slx -------------------------------------------------------------------------------- /Simulation/subsystems/Estimators.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mindThomas/Kugle-MATLAB/b1ee16de78899a131241639d620b5fb7c02a586b/Simulation/subsystems/Estimators.slx -------------------------------------------------------------------------------- /Simulation/subsystems/KinematicsIntegration.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mindThomas/Kugle-MATLAB/b1ee16de78899a131241639d620b5fb7c02a586b/Simulation/subsystems/KinematicsIntegration.slx -------------------------------------------------------------------------------- /Simulation/subsystems/KugleModel.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mindThomas/Kugle-MATLAB/b1ee16de78899a131241639d620b5fb7c02a586b/Simulation/subsystems/KugleModel.slx -------------------------------------------------------------------------------- /Simulation/subsystems/Logging.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mindThomas/Kugle-MATLAB/b1ee16de78899a131241639d620b5fb7c02a586b/Simulation/subsystems/Logging.slx -------------------------------------------------------------------------------- /Simulation/subsystems/MPC.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mindThomas/Kugle-MATLAB/b1ee16de78899a131241639d620b5fb7c02a586b/Simulation/subsystems/MPC.slx -------------------------------------------------------------------------------- /Simulation/subsystems/Madgwick.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mindThomas/Kugle-MATLAB/b1ee16de78899a131241639d620b5fb7c02a586b/Simulation/subsystems/Madgwick.slx -------------------------------------------------------------------------------- /Simulation/subsystems/QEKF_.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mindThomas/Kugle-MATLAB/b1ee16de78899a131241639d620b5fb7c02a586b/Simulation/subsystems/QEKF_.slx -------------------------------------------------------------------------------- /Simulation/subsystems/ReferenceGenerator.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mindThomas/Kugle-MATLAB/b1ee16de78899a131241639d620b5fb7c02a586b/Simulation/subsystems/ReferenceGenerator.slx -------------------------------------------------------------------------------- /Simulation/subsystems/SensorModels.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mindThomas/Kugle-MATLAB/b1ee16de78899a131241639d620b5fb7c02a586b/Simulation/subsystems/SensorModels.slx -------------------------------------------------------------------------------- /Simulation/subsystems/SlidingMode.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mindThomas/Kugle-MATLAB/b1ee16de78899a131241639d620b5fb7c02a586b/Simulation/subsystems/SlidingMode.slx -------------------------------------------------------------------------------- /Simulation/subsystems/StatesComparison.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mindThomas/Kugle-MATLAB/b1ee16de78899a131241639d620b5fb7c02a586b/Simulation/subsystems/StatesComparison.slx -------------------------------------------------------------------------------- /Simulation/subsystems/SteadyStateAccelerationModel.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mindThomas/Kugle-MATLAB/b1ee16de78899a131241639d620b5fb7c02a586b/Simulation/subsystems/SteadyStateAccelerationModel.slx -------------------------------------------------------------------------------- /Simulation/subsystems/StructurizeLoggedData.m: -------------------------------------------------------------------------------- 1 | if (exist('time','var')) 2 | sim = []; 3 | sim.time = time; 4 | 5 | sim.position = X(:,1:2); 6 | sim.velocity = X(:,7:8); 7 | sim.q = X(:,3:6); 8 | sim.dq = X(:,9:12); 9 | 10 | sim.position_est = X_hat(:,1:2); 11 | sim.velocity_est = X_hat(:,7:8); 12 | sim.q_est = X_hat(:,3:6); 13 | sim.dq_est = X_hat(:,9:12); 14 | 15 | sim.tau = tau; 16 | sim.q_ref = q_ref; 17 | sim.omega_b_ref = omega_b_ref; 18 | sim.S = S; 19 | 20 | clear time X X_hat tau q_ref omega_b_ref S; 21 | end -------------------------------------------------------------------------------- /Simulation/subsystems/TrackingComparison.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mindThomas/Kugle-MATLAB/b1ee16de78899a131241639d620b5fb7c02a586b/Simulation/subsystems/TrackingComparison.slx -------------------------------------------------------------------------------- /Simulation/subsystems/VEKF.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mindThomas/Kugle-MATLAB/b1ee16de78899a131241639d620b5fb7c02a586b/Simulation/subsystems/VEKF.slx -------------------------------------------------------------------------------- /Simulation/subsystems/Velocity_LQR.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mindThomas/Kugle-MATLAB/b1ee16de78899a131241639d620b5fb7c02a586b/Simulation/subsystems/Velocity_LQR.slx -------------------------------------------------------------------------------- /control_loop_structure.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mindThomas/Kugle-MATLAB/b1ee16de78899a131241639d620b5fb7c02a586b/control_loop_structure.png -------------------------------------------------------------------------------- /estimators.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mindThomas/Kugle-MATLAB/b1ee16de78899a131241639d620b5fb7c02a586b/estimators.png -------------------------------------------------------------------------------- /kugle_front.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mindThomas/Kugle-MATLAB/b1ee16de78899a131241639d620b5fb7c02a586b/kugle_front.jpg -------------------------------------------------------------------------------- /kugle_v1_3d.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mindThomas/Kugle-MATLAB/b1ee16de78899a131241639d620b5fb7c02a586b/kugle_v1_3d.png -------------------------------------------------------------------------------- /kugle_v1_drawing.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mindThomas/Kugle-MATLAB/b1ee16de78899a131241639d620b5fb7c02a586b/kugle_v1_drawing.png -------------------------------------------------------------------------------- /mpc_loop.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mindThomas/Kugle-MATLAB/b1ee16de78899a131241639d620b5fb7c02a586b/mpc_loop.png -------------------------------------------------------------------------------- /sensormodels.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mindThomas/Kugle-MATLAB/b1ee16de78899a131241639d620b5fb7c02a586b/sensormodels.png -------------------------------------------------------------------------------- /simulink_top_level.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mindThomas/Kugle-MATLAB/b1ee16de78899a131241639d620b5fb7c02a586b/simulink_top_level.png -------------------------------------------------------------------------------- /system_architecture.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mindThomas/Kugle-MATLAB/b1ee16de78899a131241639d620b5fb7c02a586b/system_architecture.png -------------------------------------------------------------------------------- /velocity_control_loop_structure.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mindThomas/Kugle-MATLAB/b1ee16de78899a131241639d620b5fb7c02a586b/velocity_control_loop_structure.png --------------------------------------------------------------------------------