test.cpp

#include <iostream>
#include <chrono>
#include <thread>
#include "qpOASES_interface.h"
#include "tinyMpc_interface.h"
// #include "quadprog_interface.h"
#include "osqp_interface.h"
#include "osqpE_interface.h"
#include "model_fit.h"
#include <random>

/*MPC模型参数*/
MPCFloat model_A[4*10*10]={
1.000000e+00,-1.394936e-14,5.045869e-14,-5.857560e-14,
2.000000e-03,1.099918e-17,-3.978709e-17,4.618733e-17,
0,0,0,0,
0,0,0,0,
3.938129e-05,-9.826347e-04,2.589653e-03,-2.416429e-03,
2.625163e-08,-6.550342e-07,1.726247e-06,-1.610756e-06,
3.938129e-05,-9.826347e-04,2.589653e-03,-2.416429e-03,
2.625163e-08,-6.550342e-07,1.726247e-06,-1.610756e-06,
-5.931746e-07,4.437449e-06,-1.283911e-05,1.243507e-05,
-3.954407e-10,2.958045e-09,-8.558494e-09,8.289058e-09,
0,0,0,0,
1.000000e+00,-1.394936e-14,5.045869e-14,-5.857560e-14,
0,0,0,0,
0,0,0,0,
3.939092e-02,-9.828433e-01,2.590359e+00,-2.417167e+00,
3.938129e-05,-9.826347e-04,2.589653e-03,-2.416429e-03,
3.939092e-02,-9.828433e-01,2.590359e+00,-2.417167e+00,
3.938129e-05,-9.826347e-04,2.589653e-03,-2.416429e-03,
-5.932086e-04,4.438403e-03,-1.284255e-02,1.243878e-02,
-5.931746e-07,4.437449e-06,-1.283911e-05,1.243507e-05,
0,0,0,0,
0,0,0,0,
1.000000e+00,-1.394936e-14,5.045869e-14,-5.857560e-14,
2.000000e-03,1.099918e-17,-3.978709e-17,4.618733e-17,
-8.037379e-06,-7.489500e-05,2.034609e-04,-2.018879e-04,
-5.358104e-09,-4.993023e-08,1.356404e-07,-1.345913e-07,
8.037379e-06,7.489500e-05,-2.034609e-04,2.018879e-04,
5.358104e-09,4.993023e-08,-1.356404e-07,1.345913e-07,
-1.461966e-26,1.839565e-25,-6.994732e-25,8.390681e-25,
-1.782923e-29,2.018325e-28,-7.312499e-28,8.527171e-28,
0,0,0,0,
0,0,0,0,
0,0,0,0,
1.000000e+00,-1.394936e-14,5.045869e-14,-5.857560e-14,
-8.037940e-03,-7.489414e-02,2.034617e-01,-2.018903e-01,
-8.037379e-06,-7.489500e-05,2.034609e-04,-2.018879e-04,
8.037940e-03,7.489414e-02,-2.034617e-01,2.018903e-01,
8.037379e-06,7.489500e-05,-2.034609e-04,2.018879e-04,
-2.690345e-23,3.349129e-22,-1.287868e-21,1.560692e-21,
-3.038337e-26,3.545985e-25,-1.334803e-24,1.617650e-24,
0,0,0,0,
0,0,0,0,
0,0,0,0,
0,0,0,0,
1.000482e+00,1.267101e-03,-8.900748e-03,1.148921e-02,
2.000321e-03,8.446307e-07,-5.933199e-06,7.658637e-06,
1.736110e-04,2.587719e-03,-1.168652e-02,1.374323e-02,
1.157315e-07,1.725019e-06,-7.790323e-06,9.161262e-06,
-2.925440e-06,1.632725e-06,2.077389e-05,-3.414536e-05,
-1.950183e-09,1.088576e-09,1.384730e-08,-2.276054e-08,
0,0,0,0,
0,0,0,0,
0,0,0,0,
0,0,0,0,
4.815695e-01,1.267488e+00,-8.903120e+00,1.149233e+01,
1.000482e+00,1.267101e-03,-8.900748e-03,1.148921e-02,
1.736456e-01,2.588194e+00,-1.168912e+01,1.374656e+01,
1.736110e-04,2.587719e-03,-1.168652e-02,1.374323e-02,
-2.925854e-03,1.632380e-03,2.078123e-02,-3.415670e-02,
-2.925440e-06,1.632725e-06,2.077389e-05,-3.414536e-05,
0,0,0,0,
0,0,0,0,
0,0,0,0,
0,0,0,0,
1.736110e-04,2.587719e-03,-1.168652e-02,1.374323e-02,
1.157315e-07,1.725019e-06,-7.790323e-06,9.161262e-06,
1.000482e+00,1.267101e-03,-8.900748e-03,1.148921e-02,
2.000321e-03,8.446307e-07,-5.933199e-06,7.658637e-06,
-2.925440e-06,1.632725e-06,2.077389e-05,-3.414536e-05,
-1.950183e-09,1.088576e-09,1.384730e-08,-2.276054e-08,
0,0,0,0,
0,0,0,0,
0,0,0,0,
0,0,0,0,
1.736456e-01,2.588194e+00,-1.168912e+01,1.374656e+01,
1.736110e-04,2.587719e-03,-1.168652e-02,1.374323e-02,
4.815695e-01,1.267488e+00,-8.903120e+00,1.149233e+01,
1.000482e+00,1.267101e-03,-8.900748e-03,1.148921e-02,
-2.925854e-03,1.632380e-03,2.078123e-02,-3.415670e-02,
-2.925440e-06,1.632725e-06,2.077389e-05,-3.414536e-05,
0,0,0,0,
0,0,0,0,
0,0,0,0,
0,0,0,0,
2.221661e-06,-9.507480e-05,2.751452e-04,-2.665113e-04,
1.480923e-09,-6.337775e-08,1.834105e-07,-1.776530e-07,
2.221661e-06,-9.507480e-05,2.751452e-04,-2.665113e-04,
1.480923e-09,-6.337775e-08,1.834105e-07,-1.776530e-07,
1.000021e+00,3.885357e-07,-1.244355e-06,1.254932e-06,
2.000014e-03,2.590007e-10,-8.294800e-10,8.365201e-10,
0,0,0,0,
0,0,0,0,
0,0,0,0,
0,0,0,0,
2.222352e-03,-9.509524e-02,2.752188e-01,-2.665908e-01,
2.221661e-06,-9.507480e-05,2.751452e-04,-2.665113e-04,
2.222352e-03,-9.509524e-02,2.752188e-01,-2.665908e-01,
2.221661e-06,-9.507480e-05,2.751452e-04,-2.665113e-04,
2.092300e-02,3.886225e-04,-1.244694e-03,1.255312e-03,
1.000021e+00,3.885357e-07,-1.244355e-06,1.254932e-06,
};
MPCFloat model_B[4*10*4]={
-2.116893e-06,1.757603e-04,-5.055585e-04,4.962178e-04,
-2.116893e-06,1.757603e-04,-5.055585e-04,4.962178e-04,
-2.557832e-06,-1.651063e-05,8.616224e-05,-1.049918e-04,
-2.557832e-06,-1.651063e-05,8.616224e-05,-1.049918e-04,
-2.118127e-03,1.757978e-01,-5.056937e-01,4.963643e-01,
-2.118127e-03,1.757978e-01,-5.056937e-01,4.963643e-01,
-2.558161e-03,-1.651431e-02,8.618309e-02,-1.050188e-01,
-2.558161e-03,-1.651431e-02,8.618309e-02,-1.050188e-01,
-9.363756e-06,-1.240500e-05,3.083389e-05,-2.824261e-05,
9.363756e-06,1.240500e-05,-3.083389e-05,2.824261e-05,
-1.998825e-06,1.031484e-05,-2.453817e-05,2.180230e-05,
1.998825e-06,-1.031484e-05,2.453817e-05,-2.180230e-05,
-9.363708e-03,-1.240547e-02,3.083530e-02,-2.824402e-02,
9.363708e-03,1.240547e-02,-3.083530e-02,2.824402e-02,
-1.998908e-03,1.031542e-02,-2.453973e-02,2.180378e-02,
1.998908e-03,-1.031542e-02,2.453973e-02,-2.180378e-02,
-1.330278e-04,2.082521e-04,3.830499e-04,-9.113720e-04,
-1.134670e-04,1.066494e-05,9.914720e-04,-1.535466e-03,
8.948813e-05,-5.706185e-04,1.404417e-03,-1.239203e-03,
5.157410e-05,-2.783205e-04,5.706853e-04,-4.149197e-04,
-1.330452e-01,2.082466e-01,3.833146e-01,-9.117947e-01,
-1.134837e-01,1.065309e-02,9.917575e-01,-1.535911e+00,
8.949920e-02,-5.706859e-01,1.404563e+00,-1.239311e+00,
5.158381e-02,-2.783762e-01,5.707964e-01,-4.149927e-01,
-1.134670e-04,1.066494e-05,9.914720e-04,-1.535466e-03,
-1.330278e-04,2.082521e-04,3.830499e-04,-9.113720e-04,
5.157410e-05,-2.783205e-04,5.706853e-04,-4.149197e-04,
8.948813e-05,-5.706185e-04,1.404417e-03,-1.239203e-03,
-1.134837e-01,1.065309e-02,9.917575e-01,-1.535911e+00,
-1.330452e-01,2.082466e-01,3.833146e-01,-9.117947e-01,
5.158381e-02,-2.783762e-01,5.707964e-01,-4.149927e-01,
8.949920e-02,-5.706859e-01,1.404563e+00,-1.239311e+00,
-1.559665e-06,1.520154e-05,-4.843278e-05,4.935597e-05,
-1.559665e-06,1.520154e-05,-4.843278e-05,4.935597e-05,
-4.161474e-06,2.171076e-07,3.842122e-06,-6.177251e-06,
-4.161474e-06,2.171076e-07,3.842122e-06,-6.177251e-06,
-1.559712e-03,1.520493e-02,-4.844604e-02,4.937089e-02,
-1.559712e-03,1.520493e-02,-4.844604e-02,4.937089e-02,
-4.161559e-03,2.170322e-04,3.843462e-03,-6.179297e-03,
-4.161559e-03,2.170322e-04,3.843462e-03,-6.179297e-03,
};
MPCFloat model_K[4*4*10]={
-1.881704e-10,-2.758793e-09,1.739412e-09,1.775704e-09,
-5.324526e-01,-7.796930e+00,4.883521e+00,5.084541e+00,
-1.746394e-01,-2.810808e-02,6.590327e-02,-2.894991e-02,
-8.915091e-01,-1.409036e-01,3.270970e-01,-1.348130e-01,
-2.104226e+00,-2.937667e+01,3.812587e+01,-1.455530e+01,
-2.048845e-01,-1.462424e+00,-6.094315e+00,7.652627e+00,
-2.957775e-01,-3.299242e+01,2.202584e+01,8.985211e-01,
-6.068115e-02,-8.665390e-01,-8.630793e+00,8.202723e+00,
-3.740452e+01,6.372624e+01,7.104428e+00,-8.157116e+01,
-3.513290e+00,1.068464e+01,-1.917231e+01,1.491785e+01,
-1.881704e-10,-2.758792e-09,1.739405e-09,1.775718e-09,
-5.324526e-01,-7.796930e+00,4.883521e+00,5.084541e+00,
1.746394e-01,2.810808e-02,-6.590326e-02,2.894990e-02,
8.915091e-01,1.409036e-01,-3.270970e-01,1.348130e-01,
-2.957775e-01,-3.299242e+01,2.202584e+01,8.985210e-01,
-6.068115e-02,-8.665390e-01,-8.630793e+00,8.202723e+00,
-2.104226e+00,-2.937667e+01,3.812587e+01,-1.455530e+01,
-2.048845e-01,-1.462424e+00,-6.094315e+00,7.652627e+00,
-3.740452e+01,6.372624e+01,7.104428e+00,-8.157116e+01,
-3.513290e+00,1.068464e+01,-1.917231e+01,1.491785e+01,
2.448249e-09,9.799646e-09,-4.636223e-08,4.989005e-08,
6.919139e+00,2.777494e+01,-1.313170e+02,1.412894e+02,
-1.739694e-01,5.722538e-01,7.554244e-01,-1.259700e+00,
-8.854545e-01,2.915105e+00,3.995652e+00,-6.464375e+00,
3.493778e+01,1.993582e+02,-4.079939e+02,4.249795e+02,
2.829163e+00,1.795764e+01,7.279050e+00,7.351755e-01,
-9.945330e+00,-3.151818e+01,-1.920234e+02,1.634436e+02,
-1.168966e+00,3.743866e+00,-5.126884e+01,2.968767e+01,
-3.138127e+01,-7.170407e+02,1.329132e+03,-9.016768e+02,
-1.768019e+00,-3.467285e+01,5.809701e+01,-3.646728e+01,
2.448249e-09,9.799670e-09,-4.636243e-08,4.989043e-08,
6.919139e+00,2.777494e+01,-1.313170e+02,1.412894e+02,
1.739694e-01,-5.722538e-01,-7.554244e-01,1.259700e+00,
8.854545e-01,-2.915105e+00,-3.995652e+00,6.464375e+00,
-9.945330e+00,-3.151818e+01,-1.920234e+02,1.634436e+02,
-1.168966e+00,3.743866e+00,-5.126884e+01,2.968767e+01,
3.493778e+01,1.993582e+02,-4.079939e+02,4.249795e+02,
2.829163e+00,1.795764e+01,7.279050e+00,7.351754e-01,
-3.138127e+01,-7.170407e+02,1.329132e+03,-9.016768e+02,
-1.768019e+00,-3.467285e+01,5.809701e+01,-3.646728e+01,
};
MPCFloat model_P[4*10*10]={
5.659735e-06,-2.655947e-08,7.083795e-08,-4.431127e-08,
5.098515e-06,-1.513517e-05,4.696987e-05,-4.582201e-05,
2.966872e-14,5.658334e-13,-5.904898e-12,1.166776e-11,
8.671446e-15,-5.749516e-14,5.130326e-14,1.247948e-13,
8.661064e-07,2.685072e-06,7.265166e-06,-6.201051e-06,
1.068561e-07,9.178950e-07,2.112790e-06,-1.046751e-06,
8.661064e-07,2.685077e-06,7.265132e-06,-6.200987e-06,
1.068561e-07,9.178955e-07,2.112786e-06,-1.046745e-06,
2.580862e-06,-9.735562e-06,2.356961e-05,-2.243210e-05,
1.099303e-06,-6.089013e-06,1.652373e-05,-1.600788e-05,
5.098515e-06,-1.513517e-05,4.696987e-05,-4.582201e-05,
1.445393e+04,-4.274140e+04,1.326511e+05,-1.294393e+05,
-2.230918e-07,2.783657e-06,-1.080416e-05,1.333378e-05,
-4.075289e-08,5.114492e-07,-2.007264e-06,2.502255e-06,
2.449678e+03,7.593981e+03,2.060801e+04,-1.767611e+04,
3.027164e+02,2.587915e+03,6.017507e+03,-3.025538e+03,
2.449678e+03,7.593981e+03,2.060801e+04,-1.767611e+04,
3.027164e+02,2.587915e+03,6.017507e+03,-3.025538e+03,
7.295611e+03,-2.749100e+04,6.650991e+04,-6.328303e+04,
3.107635e+03,-1.720376e+04,4.667077e+04,-4.520484e+04,
2.966872e-14,5.658334e-13,-5.904898e-12,1.166776e-11,
-2.230918e-07,2.783657e-06,-1.080416e-05,1.333378e-05,
2.041889e+02,-7.067847e-01,2.118054e+00,-9.168203e-01,
2.065299e+01,-3.592910e+00,1.072789e+01,-4.414632e+00,
3.688588e+00,-5.378965e+00,1.519424e+02,1.110795e+02,
5.433202e-01,4.571648e+00,7.388979e+00,4.978077e+01,
-3.688588e+00,5.378968e+00,-1.519424e+02,-1.110795e+02,
-5.433203e-01,-4.571648e+00,-7.388981e+00,-4.978077e+01,
-2.447569e-07,2.839707e-06,-1.049438e-05,1.237459e-05,
-3.514567e-08,4.243514e-07,-1.607252e-06,1.938908e-06,
8.671446e-15,-5.749516e-14,5.130326e-14,1.247948e-13,
-4.075289e-08,5.114492e-07,-2.007264e-06,2.502255e-06,
2.065299e+01,-3.592910e+00,1.072789e+01,-4.414632e+00,
1.078746e+02,-1.863259e+01,5.563904e+01,-2.222175e+01,
1.852871e+01,-2.989654e+01,7.618267e+02,6.311406e+02,
2.739602e+00,2.398219e+01,3.342005e+01,2.685530e+02,
-1.852871e+01,2.989654e+01,-7.618267e+02,-6.311406e+02,
-2.739602e+00,-2.398219e+01,-3.342005e+01,-2.685530e+02,
-3.511038e-08,4.125406e-07,-1.519203e-06,1.768279e-06,
-6.248679e-09,7.696597e-08,-2.974272e-07,3.647873e-07,
8.661064e-07,2.685072e-06,7.265166e-06,-6.201051e-06,
2.449678e+03,7.593981e+03,2.060801e+04,-1.767611e+04,
3.688588e+00,-5.378965e+00,1.519424e+02,1.110795e+02,
1.852871e+01,-2.989654e+01,7.618267e+02,6.311406e+02,
7.758139e+03,8.931841e+03,9.467669e+04,-1.867804e+04,
1.008485e+02,5.502190e+02,1.338429e+04,9.608830e+03,
5.343848e+02,2.504470e+03,-1.229544e+04,-2.585883e+04,
5.610455e+01,4.194095e+02,4.204462e+03,-9.493842e+03,
3.876638e+03,6.606974e+03,-1.748261e+04,1.174644e+04,
9.273980e+02,-1.894111e+03,6.170978e+03,-6.099218e+03,
1.068561e-07,9.178950e-07,2.112790e-06,-1.046751e-06,
3.027164e+02,2.587915e+03,6.017507e+03,-3.025538e+03,
5.433202e-01,4.571648e+00,7.388979e+00,4.978077e+01,
2.739602e+00,2.398219e+01,3.342005e+01,2.685530e+02,
1.008485e+02,5.502190e+02,1.338429e+04,9.608830e+03,
1.177432e+01,1.002195e+02,1.863354e+03,4.659958e+03,
5.610455e+01,4.194095e+02,4.204462e+03,-9.493842e+03,
1.235638e+01,-5.107405e+01,2.255480e+03,-1.738847e+03,
3.192076e+02,1.303192e+03,5.853828e+02,-2.191453e+03,
1.080416e+02,1.185886e+01,7.317799e+02,-7.594164e+02,
8.661064e-07,2.685077e-06,7.265132e-06,-6.200987e-06,
2.449678e+03,7.593981e+03,2.060801e+04,-1.767611e+04,
-3.688588e+00,5.378968e+00,-1.519424e+02,-1.110795e+02,
-1.852871e+01,2.989654e+01,-7.618267e+02,-6.311406e+02,
5.343848e+02,2.504470e+03,-1.229544e+04,-2.585883e+04,
5.610455e+01,4.194095e+02,4.204462e+03,-9.493842e+03,
7.758139e+03,8.931841e+03,9.467669e+04,-1.867804e+04,
1.008485e+02,5.502189e+02,1.338429e+04,9.608830e+03,
3.876638e+03,6.606974e+03,-1.748261e+04,1.174644e+04,
9.273980e+02,-1.894111e+03,6.170978e+03,-6.099218e+03,
1.068561e-07,9.178955e-07,2.112786e-06,-1.046745e-06,
3.027164e+02,2.587915e+03,6.017507e+03,-3.025538e+03,
-5.433203e-01,-4.571648e+00,-7.388981e+00,-4.978077e+01,
-2.739602e+00,-2.398219e+01,-3.342005e+01,-2.685530e+02,
5.610455e+01,4.194095e+02,4.204462e+03,-9.493842e+03,
1.235638e+01,-5.107405e+01,2.255480e+03,-1.738847e+03,
1.008485e+02,5.502189e+02,1.338429e+04,9.608830e+03,
1.177432e+01,1.002195e+02,1.863354e+03,4.659958e+03,
3.192076e+02,1.303192e+03,5.853828e+02,-2.191453e+03,
1.080416e+02,1.185886e+01,7.317799e+02,-7.594164e+02,
2.580862e-06,-9.735562e-06,2.356961e-05,-2.243210e-05,
7.295611e+03,-2.749100e+04,6.650991e+04,-6.328303e+04,
-2.447569e-07,2.839707e-06,-1.049438e-05,1.237459e-05,
-3.511038e-08,4.125406e-07,-1.519203e-06,1.768279e-06,
3.876638e+03,6.606974e+03,-1.748261e+04,1.174644e+04,
3.192076e+02,1.303192e+03,5.853828e+02,-2.191453e+03,
3.876638e+03,6.606974e+03,-1.748261e+04,1.174644e+04,
3.192076e+02,1.303192e+03,5.853828e+02,-2.191453e+03,
1.559644e+05,-4.000940e+05,9.787589e+05,-8.725527e+05,
7.671589e+03,-3.482579e+04,8.941143e+04,-8.305577e+04,
1.099303e-06,-6.089013e-06,1.652373e-05,-1.600788e-05,
3.107635e+03,-1.720376e+04,4.667077e+04,-4.520484e+04,
-3.514567e-08,4.243514e-07,-1.607252e-06,1.938908e-06,
-6.248679e-09,7.696597e-08,-2.974272e-07,3.647873e-07,
9.273980e+02,-1.894111e+03,6.170978e+03,-6.099218e+03,
1.080416e+02,1.185886e+01,7.317799e+02,-7.594164e+02,
9.273980e+02,-1.894111e+03,6.170978e+03,-6.099218e+03,
1.080416e+02,1.185886e+01,7.317799e+02,-7.594164e+02,
7.671589e+03,-3.482579e+04,8.941143e+04,-8.305577e+04,
1.106500e+03,-6.886695e+03,1.899038e+04,-1.848067e+04,
};

MPCFloat Q[10] = {
    1e-17,
    80,
    0.2,
    5,
    400,
    3.,
    400,
    3.,
    8000,
    1.7
};
MPCFloat R[4] = {
    3,
    3,
    0.1,
    0.1
};

MPCFloat F[10];
MPCFloat W[4] = {1e-6,1e-6,1e-6,1e-6};
MPCFloat lb[4] = { -4.5,-4.5,-40,-40 };
MPCFloat ub[4] = { 4.5, 4.5, 40, 40 };

int main()
{
    std::random_device rd;
    std::mt19937 gen(rd());
    std::uniform_real_distribution<MPCFloat> dist(-0.5, 0.5);

    // qpoasesInterface mpcCal(10,4,4,0,2);// qpOASES
    tinympcInterface mpcCal(10, 4, 4, 0, 3, 0.1, 1);// tinyMPC
    // quadprogInterface mpcCal(10, 4, 4, 0, 10);//qp++
    // osqpInterface mpcCal(10, 4, 4, 0, 2,0,0);//osqp
    // osqpeInterface mpcCal(10, 4, 4, 0, 20);//osqp-eigen
    // osqpeInterfaceSparse mpcCal(10, 4, 4, 0, 20);//osqp-eigen sparse

    Eigen::Vector<MPCFloat,10> X = Eigen::Vector<MPCFloat,10>::Constant(0);
    Eigen::Vector<MPCFloat,10> Y = Eigen::Vector<MPCFloat,10>::Constant(0);
    Y(1) = 1.;
    X(8) = 0.01;
    MPCFloat avc_length = 0.13;
    modelFit<10, 10, 3> fitA;
    modelFit<10, 4, 3> fitB;
    modelFit<4, 10, 3> fitK;
    modelFit<10, 10, 3> fitP;
    fitA.setFunctions(model_A);
    fitB.setFunctions(model_B);
    fitK.setFunctions(model_K);
    fitP.setFunctions(model_P);
    Eigen::Matrix<MPCFloat,-1,-1> A = fitA.modelGenerate(avc_length);
    Eigen::Matrix<MPCFloat,-1,-1> B = fitB.modelGenerate(avc_length);
    Eigen::Matrix<MPCFloat,-1,-1> K = fitK.modelGenerate(avc_length);
    Eigen::Matrix<MPCFloat,-1,-1> P = fitP.modelGenerate(avc_length);
    Eigen::Vector<MPCFloat,-1> Qv = Eigen::Map<Eigen::Vector<MPCFloat, 10>>(Q);
    Eigen::Vector<MPCFloat,-1> Rv = Eigen::Map<Eigen::Vector<MPCFloat, 4>>(R);
    Eigen::Vector<MPCFloat,-1> Fv = Qv;
    Eigen::Vector<MPCFloat,-1> Wv = Eigen::Map<Eigen::Vector<MPCFloat, 4>>(W);
    Eigen::Vector<MPCFloat,-1> lbv = Eigen::Map<Eigen::Vector<MPCFloat, 4>>(lb);
    Eigen::Vector<MPCFloat,-1> ubv = Eigen::Map<Eigen::Vector<MPCFloat, 4>>(ub);
    Eigen::Vector<MPCFloat,-1> Albv = lbv;
    Eigen::Vector<MPCFloat,-1> Aubv = ubv;
    Eigen::Matrix<MPCFloat,-1,-1> cA;
    mpcCal.mpcInit(A,B,Qv,P,Rv,Wv,0);
    while(1)
    {
        // X(8) = dist(gen);
        // X(6) = dist(gen);
        // X(4) = dist(gen);
        // 记录开始时间
        auto start = std::chrono::high_resolution_clock::now();
        A = fitA.modelGenerate(avc_length);
        B = fitB.modelGenerate(avc_length);
        K = fitK.modelGenerate(avc_length);
        P = fitP.modelGenerate(avc_length);
        // // 记录结束时间
        // auto end = std::chrono::high_resolution_clock::now();
        mpcCal.setStateSpace(A,B,0);
        cA.resize(4, 4);
        cA.setIdentity();
        // // 记录开始时间
        // auto start = std::chrono::high_resolution_clock::now();

        mpcCal.setInputConstrain(lbv,ubv);
        Eigen::Vector<MPCFloat,10> xb;
        xb.setConstant(1e17);
        mpcCal.setStateConstrain(-xb,xb);
        mpcCal.setIeqConstrain(cA,Albv,Aubv);
        mpcCal.setLqrFeedback(K,P);
        mpcCal.mpcUpdate(Y,X,5,2e-3);
        // // 记录开始时间
        // auto start = std::chrono::high_resolution_clock::now();
        mpcCal.mpcPredictionSolve();
        // mpcCal.mpcPrediction();
        // mpcCal.matrixCopy();
        // mpcCal.mpcSolve();
        Eigen::Vector<MPCFloat,-1> output = mpcCal.getOutput();
        // 记录结束时间
        auto end = std::chrono::high_resolution_clock::now();
        // 计算时间差（单位：毫秒）
        auto duration = std::chrono::duration_cast<std::chrono::microseconds>(end - start).count();
        std::cout << "runtime: " << duration << " us" << std::endl;
        std::cout << "output: " << output.transpose() << std::endl;
        std::this_thread::sleep_for(std::chrono::milliseconds(500)); // 暂停1秒
    }
    return 0;
}