SLQ Numerical Stability with mode schedule

[ghost]

The SLQ in OCS2 12.0 seems to have numerical stability issue when the mode schedule is not empty, for example the mode schedule shown below (ds means double support mode): ds ds ds ds ds ds ds ds ds ds ds ds ds ds ds 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1 1.1 1.2 1.3 1.4 1.5 and a constant state feedback (always x0), will quickly cause the SLQ riccati equation to diverge with warm start after a few iteration:

Integration terminated since the maximum number of function calls is reached. State at termination time -0.891556: [ -3.68056e+94 3.26466e+99 -2.89575e+104 -4.7569e+90 4.21937e+95 -6.14801e+86 -6.40359e+100 5.67998e+105 -8.27625e+96 -1.11412e+107 6.48803e+96 -5.75488e+101 8.38538e+92 1.12881e+103 -1.1437e+99 -2.31334e+100 2.05193e+105 -2.98985e+96 -4.02484e+106 4.07791e+102 -1.454e+106 -2.60803e+93 2.31332e+98 -3.37072e+89 -4.53755e+99 4.59739e+95 -1.63922e+99 -1.84804e+92 5.71019e+98 -5.06494e+103 7.38008e+94 9.93482e+104 -1.00658e+101 3.58902e+104 4.04622e+97 -8.85905e+102 -1.76817e+89 1.56836e+94 -2.28525e+85 -3.07633e+95 3.11689e+91 -1.11134e+95 -1.25292e+88 2.74322e+93 -8.49439e+83 -5.96575e+98 5.29162e+103 -7.71037e+94 -1.03794e+105 1.05163e+101 -3.74964e+104 -4.2273e+97 9.25553e+102 -2.86599e+93 -9.66976e+102 2.29358e+95 -2.03441e+100 2.96431e+91 3.99046e+101 -4.04308e+97 1.44158e+101 1.62522e+94 -3.55837e+99 1.10185e+90 3.71762e+99 -1.42927e+96 -3.18932e+99 2.82893e+104 -4.12201e+95 -5.54891e+105 5.62208e+101 -2.00458e+105 -2.25994e+98 4.94806e+103 -1.53217e+94 -5.16951e+103 1.98746e+100 -2.76365e+104 1.23296e+94 -1.09363e+99 1.59352e+90 2.14514e+100 -2.17343e+96 7.74948e+99 8.73667e+92 -1.91286e+98 5.9232e+88 1.99847e+98 -7.68329e+94 1.06839e+99 -4.13029e+93 -1.48445e+99 1.31671e+104 -1.91856e+95 -2.5827e+105 2.61676e+101 -9.33019e+104 -1.05187e+98 2.30304e+103 -7.13139e+93 -2.40611e+103 9.2505e+99 -1.28632e+104 4.97277e+98 -5.9871e+103 -2.01347e+90 1.78595e+95 -2.60229e+86 -3.50311e+96 3.54931e+92 -1.26552e+96 -1.42674e+89 3.12379e+94 -9.67284e+84 -3.26359e+94 1.25471e+91 -1.74473e+95 6.74494e+89 -8.12075e+94 -1.10148e+86 1.23296e+94 -1.09363e+99 1.59352e+90 2.14514e+100 -2.17343e+96 7.74948e+99 8.73667e+92 -1.91286e+98 5.9232e+88 1.99847e+98 -7.68329e+94 1.06839e+99 -4.13029e+93 4.97277e+98 6.74494e+89 -4.13029e+93 -1.48445e+99 1.31671e+104 -1.91856e+95 -2.5827e+105 2.61676e+101 -9.33019e+104 -1.05187e+98 2.30304e+103 -7.13139e+93 -2.40611e+103 9.2505e+99 -1.28632e+104 4.97277e+98 -5.9871e+103 -8.12075e+94 4.97277e+98 -5.9871e+103 2.01426e+90 -1.78665e+95 2.60331e+86 3.50449e+96 -3.5507e+92 1.26602e+96 1.42729e+89 -3.12501e+94 9.67663e+84 3.26487e+94 -1.25521e+91 1.74542e+95 -6.74758e+89 8.12393e+94 1.10191e+86 -6.74758e+89 8.12393e+94 -1.10234e+86 -6.75657e+93 5.99308e+98 -8.73246e+89 -1.17553e+100 1.19104e+96 -4.2467e+99 -4.78767e+92 1.04824e+98 -3.2459e+88 -1.09516e+98 4.21042e+94 -5.85478e+98 2.26339e+93 -2.72507e+98 -3.69621e+89 2.26339e+93 -2.72507e+98 3.69766e+89 -6.20166e+92]

If warm start is not used, it seems like the divergence will not happen. The divergence gets faster as the interval between modes decreases. The thread is set 1.